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ABSTRACT 


This work investigates the problem of robotic arm control with the goal of 
achieving given performance requirements by solving for the optimal joint 
trajectories and corresponding controls for tasks, such as point-to-point 
positioning. The resulting optimal control problem is highly nonlinear and 
constrained due to the nonlinearities in the robotic arm dynamics and 
kinodynamic constraints including limits on joint velocities and actuator torques. 

This thesis illustrates the applicability of pseudospectral methods to solve 
the optimal path planning problem for a system of multi-link, multi-degree of 
freedom robotic arms. The optimal control problem is defined in standard form 
and solved using the software package DIDO. Pontryagin’s Minimum Principle is 
used to verify that the proposed solution satisfies the necessary conditions for 
optimality. A particularly challenging aspect that is explored is the optimal motion 
of multiple arms conducting independent tasks with the risk of collision. Collision 
avoidance can be achieved by modeling appropriate path constraints. 

The processes for optimal trajectory planning are developed for a single 
two degree-of-freedom manipulator conducting point-to-point positioning and 
extended to include dual three degree-of-freedom manipulator maneuvers 
employing collision avoidance. The results demonstrate the suitability of 
pseudospectral techniques to solving the minimum time and minimum control 
maneuvers for robotic arms. The employment of collision avoidance techniques 
will facilitate continued research in autonomous robotic motion planning using 
optimal control criteria in multiple arm systems. 
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I. INTRODUCTION 


A. MOTIVATION 

The use of robotic arms has become commonplace in today’s 
technological society. These machines accomplish tasks from the mundane to 
the miraculous with little regard to how they calculate their motion by the majority 
of the populace, after all the motion control of one’s arm is much more 
complicated than a simple robotic arm yet it is accomplished with little effort. The 
reality is that the mathematical model for robotic motion is highly nonlinear and 
not intuitive. 

Optimal path planning is the primary means to achieve efficient control of 
robotic manipulators in physical space. Methods of achieving optimal control 
have been researched extensively over the past half century, but it is only with 
the increase in computational power and advanced numerical techniques over 
the past decade or so that optimal control can now be realized in a wide variety 
of tasks. The field of robotics has historically used some form of optimization to 
achieve efficiencies in the motion of systems with the goal of autonomous 
operations implied in many endeavors. 

Terrestrial applications of robots abound and the economic benefit for 
achieving efficiencies in motion cannot be taken lightly and is a driving force 
behind many of the previous studies in robotic optimal control [1,2,3]. Even a 
small increase in efficiency can have a large effect on throughput. Some of 
these applications require a form of autonomous or adaptive path planning, and 
the vast majority of them are very specific tasks that are suited to specific 
methods of calculating highly accurate trajectories that satisfy specific conditions. 
Space applications of robotic manipulators also lend themselves towards 
exploring autonomous path planning for a variety of machines. Robot arms have 
been used extensively on the Space Shuttle and the International Space Station 
(ISS) with increasing levels of complexity. A very good example of the use of 
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cooperative robotic arms was in April 2001 when the ISS’s Canadarm2 was used 
to transfer cargo directly with the shuttle Endeavor’s arm (Figure 1, [4]). While 
most of this work was accomplished via manual control, only a little imagination 
is necessary to see the usefulness of autonomous control for a system of 
cooperating robotic manipulators. The Naval Research Laboratory (NRL) has 
been conducting research in participation with the Defense Advanced Research 
Project Agency (DARPA) on a program to use a system of robotic arms on an 
advanced serving satellite [5]. SUMO, or Spacecraft for the Universal 
Modification of Orbits, is a program that is being investigated to integrate 
autonomous rendezvous and grappling technologies into a relatively low cost 
means of altering a satellite’s orbit. As Figure 2 shows, SUMO is envisioned to 
have three seven degree-of-freedom (DOF) robotic arms that will require 
advanced motion planning in a cluttered environment to operate safely. The NRL 
is developing control algorithms that operate in real time and must satisfy the 
non-linear dynamics and constraints of the system. Traditional optimal control 
techniques in the presence of obstacles were deemed “unsuitable for real-time” 
use due to their speed and complexity. 



Figure 1. Cooperating Robotic Arms in Space: International Space Station 
Arm Canadarm2 Transfers Cargo to Endeavor’s Arm April 28, 2001 (From [4]) 
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Figure 2. SUMO Advanced Servicing Spacecraft (From [5]) 


B. TRAJECTORY PLANNING 

Traditionally, the study of robotic control has been approached in two 
stages or levels (see Figure 3). The first stage is off-line path planning or 
trajectory planning. Various methods are utilized to compute a suitable state 
trajectory for the robot to achieve the desired task given the physical constraints 
on the system. Once that path has been defined, an inner loop controller can be 
used in real time to correct the errors in the path of the robot’s actual trajectory 
within the tolerance value of the desired path [6,7,8]. Most previous work has 
been focused on the path planning problem and defining some form of an 
optimal, collision free trajectory. Ideally, the computation of the optimal control 
trajectory could take place within a controller and allow a single stage control of 
robotic systems. Flowever, computation time required for the path planning 
problem typically makes this solution infeasible to implement in real time. Figure 
3 diagrams a number of robotic control methods and illustrates the two-level 
approach described [3, 8]. 
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Figure 3. Synoptic Diagram of Optimal Motion Planner for Robotic 

Manipulators (From [8]). 


The problem of trajectory planning can be formulated as a general optimal 
control problem that seeks to minimize some cost function (also called an 
objective function in some literature). Given the state variables and 

u E □ the standard optimal control problem is written in the form: 

Minimize J[x(-),u(-),ff ] = E {x{tf ),tf) + jF(x(-),u(-),f)dt 

^0 

Subectto x(f) = f (x(f),u(f),f) 

x(fo) = x° (1) 

e^{tf)<e{x{tf),tf)<e^{tf) 

h^(f)<h(x(-),u(-),0<h‘^(f) 

where J is the cost function, E is the endpoint cost, F is the running cost, f is the 
set of equations describing the dynamics of the system, e is the set of endpoint 
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constraints, and h is the set of path constraints [9]. The most common 
optimization problem in robotics is to solve the minimum time problem where the 
cost function E=tf . A running cost such as F=u^u is also used to reduce the 
control effort. Variations on this basic cost function have been explored in the 
literature [2,8,10,11,12,13]. The endpoint constraints define the final state of the 
system. The path constraints can include torque and velocity limits on the joints 
of the manipulator, geometric limits to the system, criteria for obstacle avoidance, 
and path tracking depending on the task. A trajectory is considered feasible, 
though not necessarily optimal, if the dynamics, endpoint constraints, and path 
constraints are satisfied. 

Trajectory planning for robotics typically takes one of two forms: a 
decoupled approach, which first finds a feasible path and then optimizes the 
control along that path; and a direct trajectory planning approach, which includes 
taking kinodynamic constraints of the system to solve an optimal path [7, 8]. 
These two methods each have merits and drawbacks. 

A substantial amount of research has been focused on decoupled 
methods by improving the performance of a robotic system given a specified 
collision-free path [6,7,8,13,14,15]. These types of trajectories are particularly 
useful in tasks that require specific trajectories to accomplish the assigned task. 
As an example, [13] presents an algorithm that searches for a time optimal 
control trajectory while minimizing the control effort along a specified path. This 
formulation is fairly typical of previous methods and useful as a brief illustration. 
For a robotic manipulator, the specified path is normally defined by the position 
and orientation of the tool being used. The dynamics of the system can be 
derived using a number of methods such as the Lagrange-Euler method. The 
path is first specified in terms of the generalized joint coordinates and then 
written in parametric form using a standard interpolation method. The method of 
parameterizing the motion of the robot is complex and not trivial. The path is 
scaled and its first, second, and third order derivatives are calculated. The cost 
function is then restated in terms of the scaled parametric values and minimized 
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in terms of the parametric variables, which are compared to the kinematic 
constraints of the system to check for feasibility. Reference [13] uses a genetic 
algorithm to search for the optimal solution, while reference [10] proposes phase 
plane techniques that solve the minimum time problem along a desired path 
given in parametric form. Such methods often assume some form of bang-bang 
control near the limits of the feasible joint space, which may not be desirable 
since the sudden change in manipulator torques can cause damage, or 
excessive wear, or excite flexible modes. 

When the path is not specified, approaches have been proposed to solve 
the global time optimal solution using grid or cell type iterations based on the 
decoupled path planning problem method [16, 17]. These are highly involved 
and can only be considered to converge to near-optimal trajectories in some 
cases [6]. In general, these approaches attempt to optimize every possible path 
and are therefore not practical in high dimensional problems. 

Robotic manipulators usually consist of multiple links and can achieve an 
objective maneuver through multiple paths. Optimization results often produce 
surprising results that can be physically explained, but may differ from an intuitive 
solution [3]. Full motion planning or direct trajectory planning computes the 
optimal solution in the state space of the system and solves an unknown optimal 
trajectory of each joint. When the movement of the robot is not specified, these 
trajectory methods are more useful than the decoupled path planning algorithms 
at minimizing some cost function over the semi-infinite range of possible 
trajectories. Many direct trajectory planning techniques have been explored over 
the past three decades. Excellent surveys can be found in [6], [8], and [19]. 
Figure 4 is a useful diagram that summarizes some of the methods used to solve 
the trajectory planning problems. These algorithms can be used to solve point- 
to-point, or pick-and-place trajectory problems where only the initial and final 
positions of the end effector are defined. Path constrained optimization problems 
can also be solved. Methods of computing optimal trajectories generally fall 
within two categories: direct and indirect [8]. 
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Figure 4. Optimal Free Trajectory Planning Problem MethoeJs (From [8]) 


Inctirect methoeJs are primarily basecJ on Pontryagin’s Minimum Principle 
ancJ solving the necessary coneJitions for optimality [18,19]. Given a standard 
problem formulation of the form of Equation (1), the control Hamiltonian is 
defined where/-/ e □ and A e □ is the adjoint covector or costate vector: 

/-/(A,x,u,f) := F(x,u,f) + A^f(x,u,0 (2) 

Then applying the Lagrange multiplier, p eD in order to define the Lagrangian 
of the Hamiltonian yields: 

H(|j,A,x,u,f) = H + iJ^h (3) 
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Similarly, an end-point Lagrangian is given by 

£ = E + v^e 


( 4 ) 


where veD The lower Hamiltonian, M\s the value of the Hamiltonian with 
the optimum control profile and is defined as 

M{K,x,t) = (5) 


The necessary conditions can then be described by Equations (6)-(10) below [9]. 

8H 

Hamiltonian Minimization Condition: — = 0 (6) 

0 U 

8H 

Adjoint Condition: -A = — (7) 

dx 


Transversality Condition: 


Hamiltonian Value Condition: 


A(fo) = 


Ktf) = 


dE 

dx{t,) 

dE 

dx{tf) 


( 8 ) 


H{to) = 


H{tr) = 


dt, 

_d^ 

dt, 


( 9 ) 


Hamiltonian Evolution Condition: M = — (10) 

dt 


The optimal trajectory can then be found by solving the multi-point 
boundary value problem by a multiple shooting method though convergence to a 
solution can be difficult to predict [8]. An excellent survey of indirect methods for 
computing the optimal trajectory for robotic manipulators is presented in [19]. 
The penalty for applying indirect methods to solve the optimal control problem is 
the amount of effort required to deriving the necessary conditions, particularly for 
complex systems. 
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The most common method to solving optimal control problems in recent 
years is by using direct methods [6,8,15,19]. In general these methods depend 
on discretizing the state and control variables and using nonlinear optimization 
techniques [15], evolutionary or hierarchical approaches [20,21], or stochastic 
methods to optimize the solution. The latter two techniques tend to suffer from 
“numerical explosion when treating high dimension problems [8].” 

Almost all the methods to solve optimal problems require a level of 
discretizing the state and control parameters in some way. Traditional 
parameterization is done by uniformly distributing nodes along the time profile 
and solving a constrained optimization problem using gradient based nonlinear 
optimization techniques such as sequential quadratic programming (SQP). An in 
depth approach to treating the optimal control problem via a direct means is 
presented in [3] and is representative of nonlinear optimization techniques. An 
initial control function is used to provide an initial guess to the algorithm within 
the feasible bounds of u. The corresponding state values, x, are then 
approximated using a collocation technique that results in a piece-wise trajectory 
that satisfies the system dynamics and constraints via a multiple shooting 
method. The state and control function can then be parameterized and used to 
define the cost function. This results in a large scale nonlinear optimization 
problem. A numerical algorithm, such as SQP, which is tailored to the structure 
of the problem, is then used. 

Regardless of the techniques used to solve the path planning problem, the 
direct and indirect methods can be used to complement each other. A candidate 
solution can be verified by comparing the results to the necessary conditions for 
optimality [22]. A hybrid solution is presented in [2] that uses a direct collocation 
approach, which parameterizes the state, and control variables with a poor initial 
guess trajectory using the endpoints to interpolate a discreet solution. This is 
then used to approximate a nonlinear optimization problem using SQP. 
Pontryagin’s necessary conditions are then calculated symbolically based on the 
dynamics and constraints of the system. The results of the direct collocation 
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calculation can then be used as the initial guess to solve the multipoint boundary 
value problem. While this method is useful, the calculation of the necessary 
conditions are computationally intensive and make solving anything more 
complex than a 3-DOF system difficult [2], 

Pseudospectral methods have also been proposed to solve the trajectory 
planning problem for robotic manipulators [23]. This method differs from 
previous direct methods primarily by the discretization method. The Legendre 
pseudospectral method approximates the state and control variables using 
particular interpolating polynomials. The discretized nodes are non-uniformly 
spaced based on Legendre-Gauss-Lobatto point allocation. The Covector 
Mapping Theorem allows this method to “make no distinction between the so- 
called direct and indirect methods” [22] and lends itself to verification by 
application of Pontryagin’s Minimum Principle with the automatic generation of 
states, costates, and other dual variables. 

C. THE COOPERATIVE PLANNING PROBLEM 

Multiple robots working in the same space complicate the path-planning 
problem exponentially. There are generally two traditional approaches to 
attacking the problem of coordinated movement between multiple robots: 
centralized planning and decoupled planning [6]. The latter consists of finding 
feasible paths for each robot without regard to the other robots, in other words 
disregarding any risk of collision. The robots are then sequenced by adjusting 
start times and velocities of the individual arms to avoid potential collisions 
[24,25]. This method may be considered attractive from a computational aspect 
because the dimensions of the problem are limited to a single robot at a time; 
however, the system is not optimized. The former robots become solid obstacles 
in the way of the latter’s motion and may shrink their feasible work space to zero. 
Cooperation between the robots is unlikely. 

Centralized planning computes the trajectory of all elements of the system 
simultaneously and effectively treats the system as a single state vector. As the 
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number of individual robots increase, the dimension of the problem increases 
accordingly. When the centralized planning problem is solved, the solution 
provides the state and control trajectory for the complete system. While 
computation speed may seem a concern in implementing the method on practical 
applications, increased computing power and efficient numerical techniques are 
allowing centralized planning of more and more complex systems to be plausible. 

Obstacle avoidance algorithms rely on measuring the distance between 
the objects and including some buffer. A number of methods that maneuver a 
point target around an obstacle-rich environment have been previously proposed 
[26,27]. Distance functions are used in most algorithms that use geometric 
shapes to define the obstacles, most of which compare the set of points for one 
obstacle to the set of points in the other obstacle and finding the minimum 
distance. This requires some means of discretizing the obstacles and using a 
brute force method to compare each point [6,28]. For example using a finite 
number of different radii spheres to cover the obstacles and constrain the 
minimum distance between spheres [8]. This formulation would define a path 
constraint such that at each point in time the minimum distance between the 
centers of each sphere is greater than some reference distance. 

A variation of this technique was presented in [29] where a distance 
function from a set of reference points of each manipulator to the obstacle was 
computed. 


D,(K.Pl) 
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where Dk is the distance, is a set of 1= 1,...,n points that describe the contour 

of the manipulator arms at time k, is a set of points that describe the center of 

each obstacle, i, at time k, and Lj describes the obstacle size in each direction. 
The cost function can then include the term Jd where J° is a reference distance: 
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A collision, in theory, would make Jd=0 which is the maximum value in this 
formulation. A collision free path, assuming the function is properly scaled, 
would have a lower cost than a path with a collision. However, there is no 
guarantee that the system will avoid a collision. 


D. A COOPERATIVE CONTROL PROPOSAL 

Optimal cooperative path planning consists of simultaneously solving 
trajectories for multiple robotic arms while meeting all obstacle avoidance criteria. 
Rather than discretizing the links of an arm and solving the avoidance problem in 
the work space, this thesis proposes reformulating the minimum distance 
problem in a parametric form and solving a parameterized optimization problem. 
Pseudospectral optimal control formulations readily lend itself to solving such 
problems. 

Using the software program DIDO, pseudospectral methods are used to 
solve the optimal minimum-time trajectory for 2-DOF and 3-DOF robotic 
manipulators conducting point-to-point maneuvers where the initial point is given 
in joint coordinates and the final point in Cartesian coordinates. The computed 
state, costate, and control trajectories facilitate verifying Pontryagin’s necessary 
conditions for optimality. The calculated control trajectory is then propagated to 
demonstrate feasibility. Building on these results, dual manipulator solutions are 
analyzed and presented using variations of cost functions. Finally, two methods 
of formulating obstacle avoidance criteria are presented and compared in various 
scenarios. A static obstacle avoidance formulation is first illustrated followed by 
a system of two multiple link arms performing a simple pick-and-place operation. 
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II. OPTIMAL TRAJECTORY PLANNING 


Pseudospectral (PS) methods have been used to solve a variety of 
trajectory planning and optimal control problems as shown in [27,30,31,32,33]. 
PS methods have been flight tested onboard the International Space Station, and 
other flight experiments are in the planning stages. Given their widespread 
applicability, it should be no surprise that the manipulator path planning problem 
is a logical application of these techniques. Motion planning for time-optimal 
point-to-point maneuvers in a pick-and-place operation is considered here. The 
optimal control formulation incorporates realistic constraints on the joint velocities 
and accelerations as well as bounds on actuator torque to ensure the solution 
trajectories are physically realizable. The efficacy of PS optimal control 
techniques is demonstrated via simulation. The first analysis considers a simple 
two link, two degree of freedom (2-DOF), three-dimensional motion of an arm 
with a full development of kinematics, dynamics, constraints, and Pontryagin’s 
necessary conditions [18]. The second analysis uses the same principles to 
develop trajectories for a single three link, 3-DOF manipulator. 

A. PROBLEM FORMULATION FOR A 2-DOF MANIPULATOR 

The robotic arms used throughout this study are based on the Cyton 
Alpha 7D1G from Robai. While not exact, the model assumes homogenous rigid 
arms and perfect actuators. Dynamics that are more complex can be 
incorporated into the problem formulation with the same techniques 
demonstrated below used to formulate the optimal control problem. 

1. Modeling Dynamics and Kinematics 

The Cyton Alpha is a seven degrees of freedom manipulator with an end 
effector as pictured in Figure 5 [34]. Specific joints can be locked to simulate 
fewer degrees of freedom. The first problem formulated is a three dimensional 
motion of a 2-DOF arm as sketched in Figure 6 where a-, is the offset in the local 
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X coordinate system and di is the offset in the local z direction. 0i is the angle of 
rotation of the base and 02 is the angle of the arm with respect to horizontal. 
Based on the Cyton Alpha specifications, limits were placed on the motor torque 
for each joint, Timax and the maximum angular velocity, coimax- Initial discrepancies 
between the model dynamics and the actual arm in the laboratory required 
angular acceleration limits, Oimax in order to increase the accuracy of the results. 
Link 1, the base unit, is modeled as a homogeneous cylinder and the arm is 
modeled as a thin rod. 



Figure 5. Cyton Alpha 7D1G 7-DOF Robotic Manipulator (From [34]) 



Figure 6. Sketch of 2-DOF Robotic Arm Model 
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Link 1 (Base) 

Link 2 (Arm) 

8 ^ 0.00 m 

di 0.15 m 

mi 0.150 kg 

Timax 2.4 N m 

Wimax 7.5 rad/sec 

Climax 7.5 rad/sec^ 

82 0.48 m 

62 0.02 m 

m 2 0.385 kg 

T2max 2.9 N m 

W2max 7.5 rad/sec 

c(2max 10 rad/sec^ 


Table 1. 2-DOF Dimensions and Limits 

It is usually desirable to map the position of the end effector from joint 
coordinates to a Cartesian coordinate system. The origin for the coordinate 
system is the center of the base. A Denavit-Hartenberg homogenous transfer 
matrix was used to characterize the kinematics of the robotic arm. Using planar 
rotations and linear displacements Equations (11) and (12) explicitly define the 
end effector position. 

cos0^ -sin6>| 0] (( rcos6>2 0 -sin6>2ira2y 

sin6>| cos6>| 0 -d^ +01 0 0 (11) 

0 0 1 y c/^ J sin6>2 0 cosO^ toj^ 

Xg = cos 6 ^ + c /2 sin 6>| + 82 cos 0 ^ cos O 2 

=aiSin6>| -c/2COs6>2 +82 sin6>| cos6>2 (12) 

Zg = 6^+82 sin 6>2 

While this is the method employed almost universally and will be used 
throughout this work, other means of deriving the kinematics are available and 
may prove to be better suited to solving the optimal control problem. Reference 
[15] has suggested the use of Lie groups and Lie algebras to formulate the 
kinematics and subsequent dynamics of the system. 

The dynamics of the robotic arm were calculated using a Lagrangian 
formulation of the equations of motion [7,35,36]. The general equation of motion 
takes the form 

D(q)q + C(q,q)q + g(q) = Q (13) 
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where q is the vector of generalized coordinates (0i and 02 in this case), D is a 
positive definite inertia matrix, C is the velocity coupling vector, g is the 
gravitation force vector, and Q is the generalized force vector or the joint torques, 
xi and t 2 in this case. The algorithm presented in [35] was used to develop the 
general equations for this model symbolically using the software program Maple 
and is presented in the appendix. Using the values from Figure 6 , Equation (13) 
was solved and is shown to three significant figures for the 2-DOF three- 
dimensional model in Equation (14). 


0.0128+ 0.0126 cos 26*2 

-0.00183 sin 26*2 


-0.00183 sin 26*2 

0.0291 



-0.0291^^4 sin 26*2 -0.001834 cos 6*2 
0.01464 sin26*2 


0 

0.899 cos 6*2 




\^2j 


(14) 


2. Formulation of Optimal Control Problem 

The dynamics formulated above are the basis for the optimal control 
problem formulation shown in Equation set (15). Let x be the state vector and u 
the control vector. The cost function, J, was chosen to minimize the time of the 
maneuver based on the constraints of the system. Other cost functions can be 
developed to minimize the control effort (such as a quadratic cost), energy, or 
some combination of elements, e is the endpoint function where the initial angles 
(P and the final endpoint coordinates [x^, /, z^] are given by solving Equation (12) 
for a feasible 6*1 and 02 - In addition, let h be defined as the path constraints on 
the function that in this case is simply the angle, angular velocity, and torque 
limits. 
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X = 


■^r 


:-90° <90°' 

02 

eX:=< 

^ 2 :45° <^2 <135° 

4 


0, : -7.5 < < 7.5 

02 


4 :-7.5<4 ^7.5 




eU:= 


r t,:-2A<t,<2A] 
: -2.9 < r 2 < 2.9j 


Minimize J[x(C),u(r),ff ] = 


Subject to; x(0 = 


4 

a 


x(^o) = 


.^2 

4 

4 

0 

0 


CO, 


COo 


a. 


«o 


where «/ =/'(x,u) 


e(Xf,f,) = 


cos 6*1 + dj sin cos 0 ^ cos 0 ^ - 
a^ sin 6*1 - 0^2 cos 0 ^ + 82 sin 0 ^ cos 0 ^ - 

+82 sin 6*2 -z'^ 


CO, 


COo 


(15) 


[ 0 ] 


fx'-l 


[x^l 

_ .L 

<h(x,u)< 

_ .u 

u 


u 


3. Necessary Conditions 

To be optimal, the solution must satisfy Pontryagin’s necessary conditions. 
Define the Hamiltonian, H as a function of running cost, F, the vector of costates, 
A(t), and the right-hand side of the dynamics, x(f) = f(x,t/,f): 

H(A,x,u,f):=F + A^f ; AeD^'x 

H — co^ + CO 2 + cx^ + CC 2 (^ 6 ) 

The path constraints must be taken into account as well. For the simplest 
2 DOF problem the constraints are the limits of the control and state vectors 
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defined in Equation set (1515). Equation (17) defines the Lagrangian of the 
Hamiltonian, H. 




M 




H = //. 0 , + //. 6»2 + (/I. + u ) 0 ), +{\+ //„ )co^ + «1 + 4 , «2 + Mr, + Mr, 


(17) 


Similarly, the Equation (18) defines the Endpoint Lagrangian, E where E 
is the endpoint cost: 

E(fo,f,,Xo,x„v):=£ + v^e; veD'"' 

E = v>o + o,fXf + u^Yf + + v> 


Pontryagin’s necessary conditions are defined in Equations (19) - (23). 

dH 

The Hamiltonian Minimization Condition (HMC) requires — = 0 

du 


du 


' dH^ 


dr^ 


dH 


ydTi) 

\ 


da^ da^ 

A -h A -h u, 

dr^ dr^ 

da^ da^ 

A -h A -h jU 


5u 


1 14600 1 917 sin ^2 
Den Den 

T^917sin6’2 (7390+ 7280cos 26 * 2 ) 

' +— -=- + Mr. 


Den 


Den 


= [ 0 ] 


Den = 214 + 212COS 26*2 +1 . 68 cos^ 6*2 


(19) 


The adjoint condition requires -A 


dx 
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. da^ . 5«2 

A -h A -h jU/) 

80^ ' 

- - 5«2 

A -h A-h 

®1 ;5/3 ®2 a/3 '^t '2 


/I X 1 -1 

( 3 „ +„ )+3 ^ + 1 ^ 

^ 5^2 


(^1 +/^®, ) + 4 


.■.+ A 


i\ +/^®S ) + 4 


/ 0 \ '-^'^1 / 0 \ '^'^2 
(/^, + U )-h ( /I + /^ )- 1 " Uft 

' £i>| “a, ' Q0 ' l »2 ““2 ' Q0 

sin(26'2)(252(y2 -15.9sin(6'2)(»i) 

127 + 126cos(26»2) + cos"6 »2" ^ 
sin(26'2)(15.9sin(6'2)(2>2 -128(»i -126(»i cos 26 * 2 ) 
127 + 126 cos( 2 ^ 2 ) + cos" 6 » 2 " 
sin(26'2 )(252(2;2 -15.9sin(6'2^) 

"" 127 + 126cos(2^2) + cos" 6 » 2 " ^ 

sin( 6 ' 2 )( 15 . 9 sin( 26 ' 2 )(»i + 2 cos( 6 ' 2 )(» 2 ) 
127 + 126 cos( 26 » 2 ) + cos' 6 » 2 ' 


It is valuable to note that A 01 is constant and can be used to verify the 
optimality conditions. The transversality condition requires 

A(f,) = ^ Kk) = -^ 


A(fo) = -v. 


Ktf)= ^xf 


dXf dVf dZf 

dXf By. dZf 
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u^f (-0.476 sin 6?, cos 02 + 0-02 cos 6() + --- 

+ (0.476 cos 6 '^ cos 6*2 +0.02 sin 60 

y^O“0-47’6sin6' sin6'2) + --- 

••• + y^^ (-0.476 sin 6 ?, sin 6 ' 2 ) +(0.476 sin 6 ' 2 ) 


The Hamiltonian Value Condition requires: 

H{t,) = ~ = --\ ( 22 ) 

8tf 

Finally, the Hamiltonian Evolution Equation requires that the optimal 
control trajectory satisfies Equation (23). 

H = — = 0 (23) 

dt 

The above necessary conditions provide two useful checks for optimality. 
The Hamiltonian can be analyzed for optimality and should be a constant -1 for 
all time and Aei is constant. 

4. Minimum Time Simulation Set-up and Results 

The three-dimensional 2-DOF robotic arm is used to demonstrate the 

applicability of using pseudospectral techniques to solve the time-optimal 

trajectories with various endpoint conditions using the software package DIDO. 

Two trajectories are presented below. To ensure that the endpoint coordinates 

are reachable within the state bounds, feasible initial and final angles were first 

selected. The final angles were then mapped to the corresponding final 

manipulator endpoints using Equation (12). The assumption in this the simplest 

case is that the final orientation of the manipulator is not a constraint on the 

system, only that it is at the correct position. It is important to note than even in 

the 2-DOF system, a given set of joint angles produces a unique point in space, 

but the reverse is not necessarily true. A point in space may have multiple 

20 





solutions in joint space and choosing specific final angles to ensure the feasibility 
of the maneuver does not guarantee that those angles would be the optimal final 
state. 

To obtain the optimal solution, DIDO was run in normal mode using a 
bootstrapping technique. The initial run was calculated using a 16 node solution 
that in turn was used as a guess for a 30 node solution that in turn was used as 
an initial guess for a 60 node solution. It was found that 60 nodes were sufficient 
to properly propagate the system using a differential solver such as ODE45 and 
using the linearly interpolated values of the discrete control trajectory. 

Table 2 lists the initial conditions used for two scenarios. The final angles 
were used to calculate the final endpoint constraints. Scenario 1 consisted of a 
positive 90° rotation about the base of the arm with the initial and final angle of 
the second link at 45°. Figure 7 shows the optimal trajectory results of that run. 
As shown, the time-optimal maneuver was calculated to be a simple rotation 
about the base. Figure 7 - Figure 9 plot the results for analysis. As shown, H=-1 
and Aei is constant as required by Equations (23) and (20). The minimum time 
maneuver completes in 0.92 seconds for this problem formulation. 


2-DOF Simulation Scenario 1 

2-DOF Simulation Scenario 2 

0° =0 

0° = 45° 

o to 

O) 

II II 

X, = 0.020 

y, =0.337 

z, = 0.483 

to 

o 

II II 

O o 

<J> LO 

1 

II II 

T— CM 

Qi 

x, =-0.020 
Yf =-0.337 
z, = 0.483 


Table 2. 2-DOF Simulation Initial Conditions 
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Angular Rates [rad/s] Angles [rad] 


(((=0 

(,(=45 



Xf = 0.020 m 
Yf = 0.337 m 
Zf= 0.483 m 


Figure 7. 2 DOF arm simulation, Scenario 1: 90° maneuver. 
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Figure 9. 2-DOF Arm Simulation, Scenario 1: Costates and Flamiltonian 


Scenario 2 was designed as a similar maneuver with the arm rotating -90° 
about the base. Again, the final coordinates of the manipulator end effector were 
calculated using Equation (11). The results were quite different than the previous 
scenario as shown in Figure 10 with a maneuver time of 0.880 seconds. Figure 
11 shows that the final base angle, 0i, is not -90° as expected but rather 83.2° 
(1.45 radians) and the arm elevation, 02, is 135°. No assumptions were made as 
to the trajectory of the arm and it became obvious that the endpoint coordinates 
could be achieved in two ways and solving the reverse kinematics problem 
shows this. The pseudospectral solver solved the solution that required the 
minimum time to accomplish, which in this case was a rotation of 83.2° vice a 
rotation of 90° due to the offset of the arm from the centerline of the base. 
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Figure 10. 2-DOF Arm Simulation, Scenario 2: “-90°” maneuver. 
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The control torques on both joint actuators were indirectly constrained in 
the simulation by an added constraint on the angular acceleration based on 
empirical performance characteristics measured on a physical arm. 
Nevertheless, the pseudospectral solver produced a control that resembles a 
bang-bang control for both actuators as shown in Figure 12. However, the 
torque required to rotate the base was noticeably smaller. The momentum of the 
arm in combination with the small base torque, was sufficient to rotate the arm 
the 83.2° required to complete the maneuver. 



time 


Figure 12. 2-DOF Arm Simulation, Scenario 2: Control Torque 


To ensure the feasibility of the trajectory, the control torque was 
interpolated using a basic linear interpolation function in Matlab and propagated 
using the same dynamic equations (14) and the ODE45 function at 20 Hz 
intervals (the minimum sampling rate of the arm). The results are plotted in 
Figure 13 with the propagated angles and angular velocities overlaid every 0.05 
seconds on the DIDO solution. 
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Figure 13. 2 DOF Arm Simulation, Scenario 2: Propagated State Vector. 

While relatively simple dynamics were used in the above problem 
formulation, the method can be used on more complex dynamics that include 
actuator characteristics and non-rigid links [37]. The PS methods implemented 
by DIDO would refine the optimal solution as the accuracy of the model 
increases. 

B. PROBLEM FORMULATION FOR 3-DOF MANIPULATOR 

1. Modeling Dynamics and Kinematics 

The Cyton Alpha arm was also modeled as a 3-DOF manipulator. Figure 
14 is a sketch of the configuration used in deriving the equations of motion. The 
angle of the bottom link was locked at 45°. Maple was used to map the joint 
positions and end-effector position from link coordinates to Cartesian coordinates 
similar to the 2-DOF case above. Equation (24) defines the positions of the arm 
end-effector as functions of 0i. For the purposes of this simulation, the two free 
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links have dimensions given in Table 3 and are joined by simple revolute joints 
with the angular limits defined by Equation set (25). The Equations of motion for 
this model were developed using the algorithm presented in [35] in form of 
Equation (13) using Maple. 



Figure 14. 3 DOF Robotic Arm Configuration 


Link 1 (Base+Arm 1) 

Link 2 

Link 3 

ai 0.115 m 

di 0.270 m 

mi 0.375 kg 

a 2 0.170 m 

d 2 0.020 m 

m 2 0.100 kg 

as 0.140 m 

ds 0.010 m 

m 3 0.110 kg 


Table 3. 3-DOF Arm Dimensions 


Xg = cos 6 !| + (c /2 + c/ 3 ) sin 6 !| + 83 cos 6 ' cos 02 + ^3 cos 6 ' cos( 6'2 + 0 ^) 

sin 6 !| - (c/2 + c/3 ) cos 0 ^ + 83 sin 6 ' cos 02 + 33 sin 6 * cos( 6'2 + 0 ^) (24) 

Zg = c/i + 83 sin 02 + a^ sin( 6'2 + 0^) 
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2. Formulation of Optimal Control Problem and Necessary 
Conditions 

The optimal control problem builds on the previous 2-DOF example. With 
the state and control variables, x and u respectively, defined below, the minimum 
time problem formulation is shown in Equation set (25). The G, and n limits were 
defined in the Cyton documentation. In order for the links to be kept rigid by 
intermediate motors, angular acceleration limits, Oi were imposed on the system 
based on actual tests conducted on the arm. It is important to note that 9. = «. in 
the inertial reference frame. While not ideal, the above described formulation is 
to demonstrate the feasibility of the pseudospectral methods and an exact model 
was deemed beyond the scope of this investigation. 


X = 


eX:=< 

' 0 ^ : -90° < 0 ^< 90° ' 

^2 :-40° <^2 ^'130° 

6*3: -85° < 6*3 < 85° 

u = 

To 

eL/:=< 

:-2.4<ri <2.4' 
Tj : -2.9 < r, < 2.9 - 


4 

4J 


9 , : -7.5 < 9 ,< 7.5 
9 ^-. -13 < 9 ^ <1.5 
4 : -5.8 < 4 - 5.8 


z 

J3_ 


z z 

r3 : -0.5 < ^3 < 0.5 


Min 


7[x(q),u(n),r^ 


Subject to: x = 


= t, 


XUn = 


I 
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I 

I__ 

4 



4 

_ 

O3 

<b^ 





(X2 

<b^ 



4 

4 
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where a. = /(x,u) 


0 0 o]' 


(25) 


28 



e(X/,?/) = 


x„ - X 


ye-y 

f 

z, -z 


CO 


coi 


coi 


[ 0 ] 


V 


V 


-10 < < 10 

u" 

<h(x,u)< 


where 

-7.5 < ^2 - ^-5 

a" 




-7.5 <a^< 7.5 


Just as in the 2-DOF problem, Pontryagin’s necessary conditions for 
optimality were derived for the 3-DOF arm. These conditions are similar in form 
to Equations (16) - (23). The only two useful results of the necessary condition 
analysis are that the H=-^ for all time, t and Aei is constant. 


3. Simulation Results 

A number of simulations using this problem formulation were conducted 
using DIDO. For each simulation, the initial angles and the final manipulator 
coordinates were defined. The final position of the robotic end-effector was 
chosen such that the final joint angles were at their upper limits. Figure 15 
shows the results. Just as they were calculated in the 2-DOF model, the 
manipulator coordinates were calculated using Equation (24) given a set of 
feasible joint angles. The results are similar to those from the 2-DOF problem. 
Figure 16 and Figure 17 show the computed state and control trajectories for this 
problem. 
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Figure 15. Single Arm 3 Degree of Freedom (3-DOF) Optimal Time Maneuver 
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Figure 16. Single Arm 3-DOF Simulation State Trajectory 
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Figure 17. Single Arm 3-DOF Simulation Control Trajectory 

The increase in complexity from the 2-DOF to 3-DOF problem is evident 
from the time duration of the pseudospectral calculations. The runtime for the 2- 
DOF problem averaged a few seconds, while the 3-DOF problem took about one 
minute. More importantly was that DIDO required an initial guess for some of the 
endpoint conditions in order to find a feasible solution. For this problem, a two 
point guess was used with the initial and final angles and a final time of 2 
seconds. The angular velocity and control torque were assumed to be 0 for the 
guess. DIDO was able to solve the optimal control problem and find a feasible 
solution. Future versions of DIDO will incorporate more advanced guess-free 
algorithms such as those discussed in [38] that may alleviate this requirement. 

C. DUAL ARM TRAJECTORY PLANNING 

The ultimate goal of this investigation is the simultaneous path planning of 
multiple robotic manipulators. As such, the next step taken was to formulate the 
minimum time problem using two arms and analyze the results. Below is the 3- 
DOF problem formulation with simulation results from both 2-DOF and 3-DOF 
arms. No attempt was made to avoid arm interference, but only to demonstrate 
the ability for pseudospectral methods to calculate dual arm trajectories and 
compare to single arm trajectories. 
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1 . 


Problem Formulation 


The dynamics and endpoints of the multi-arm problem are formulated 
similarly to the single arm problem. The dual arm formulation is presented below 
for the 3-DOF robotic motion case. In this case, the arms are identical copies of 
each other, but this does not need to be the case. Equation Set (26) defines the 
endpoint coordinates for each arm a and b. Arm b is offset from arm a and the 
origin by the vector . 


Xg = a^ cos 0^^ + (^2 + dg) sin 0^ + 82 cos 0^ cos 01 + cos 0^ cos(6'2 + 0 ^) 
yI = a^ sin 0^ - (^2 + dg)cosdf + 82 sin df cos 01 + sin d® cos(d2 + 0 ^) 

Ze' = d, + a 2 sin 0^ + a^ sin(d 2 " + 0^) 

xf = Xg + a^ cos 0 ^^ + (d2 + dg)sin 0 ^ + a2 cosd* cos 02 + a^ cosd* cos(d2* + 0 ^) 
Ye = Vo + d-\ sin 0 ^ - (d2 + dg)cosd* + a2 sin d* cos 02 + a^ sin 0 ^^ cos(d2^ + 0 ^) 
zl =d^+ a2 sin 02 + ag sin(d2'’ + 0 ^) 


Equation sets (27) and (28) show that the dynamics of the problem are 
completely uncoupled in its formulation. 


X = 


^2 

^3 

O' 

O' 

ab 


CO. 


CO. 


CO. 


CO. 


d,: 

-90° < di < 90° ' 


< 

d2: 

-40° <d2 <130° 


< 

dg: 

-85° < d3 < 85° 

> u = 


<».,: 

-7.5 < »., < 7.5 


CO 2 : 

-7.5 <o}2< 7.5 


^2 

: 

-5.8 <co^ < 5.8 


b 

L^3 J 


-2.4 <T,< 2.4 
-2.9 ^ ^2 < 2.9 > 
-0.5 < Tg < 0.5 


(27) 


The optimal control problem is now defined as follows: 
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Minimize 


J[x(r),u(D),ff] = ff 


Subject to: x(t) = 




1 

CD 

3 

1 __ 



w'’ 



a" 

1 

£• 

Cr 

1 _ 


1 

fi 

Cr 

1 _ 


where a' =f(0',w' ,T' ) 


(to) = [^r 02° 0f 0'° 02° 


e{Xf,tf) = 


x: -X 


2 
af 

y: 

z: 

_ ^bf 

.,ib .,af 


zf-z- 

cof 

o4 


(y. 


(O. 


,bf 




,bf 


COr, 


,bf 


[ 0 ] 


^3° 

0 0 0 

X^ 

r 

u^ 

<h(x,u)< 



a 





a 


where 


- 10 <«; <10 

-7.5 < a' < 7.5 
-7.5 <al< 7.5 


0 ]^ 


(28) 


2 . 


Simulation Results 


Adding the second arm increases the complexity of the problem and for 
some endpoint conditions, a simple two-point guess was required for DIDO to 
converge to a solution. A bootstrapping technique was used with a 16 node 
solution being calculated then used as a guess for a 30 node and then a 60 node 
solution. The second arm was offset from the origin by 30 cm in the -y direction. 
Figure 18 plots the simulation results for the two arms with the given endpoint 
conditions. Figure 19 plots the Hamiltonian for the system and verifies the 
necessary Hamiltonian Value Condition. 
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Figure 18. Dual Arm 3-DOF Simulation (No Obstacle Avoidance) 
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Figure 19. Dual Arm 3-DOF (No Obstacle Avoidance) Flamiltonian 
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These results were derived using minimum time as the cost function. 

While all the joint angles are coupled, generally there is a joint that limits the 

minimum time, in this case (h. for arm A and 6*1 for arm B. In addition, one arm 

will limit the minimum time of the entire multi-arm system. It is important to note 

that the minimum time for the system to complete the maneuver is dictated by 
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the arm whose time-optimal maneuver takes longer to complete. The trajectory 
of the other arms and joints will technically meet the minimum time condition 
regardless of the path it takes so long as it arrives at its endpoint conditions at tf. 

Figure 20 plots the differences that can occur between the dual arm and 
single arm minimum time maneuvers. The solid lines in the figures are the single 
arm solutions and the dashed lines are from the dual arm system. The minimum 
time defining joint O 2 for arm A is nearly identical for both the single and dual arm 
algorithms. The other joints, while not following exactly the same trajectory, did 
find the same minimum time route to the endpoint. Arm B, which took 0.92 
seconds to complete its maneuver in the single arm computation follows a similar 
path in the dual arm algorithm, but at a slower rate. 
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Figure 20. Comparison of Dual Arm Algorithm and Single Arm Algorithm 


While technically the results are time-optimal, there is no measure of 
efficiency in the computation of the angles of arms that are not limited by the 
system minimum time. A good example is the oscillations in arm B’s and 02 in 

the dual arm algorithm. One remedy is to solve for the optimal trajectory in two 
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steps. First, solve the minimum time problem, then use that minimum time to 
bound the horizon of the problem and solve a minimum energy or quadratic 
problem among the various possible minimum time trajectories. Another solution 
is to add some measurement of efficiency such as a minimum energy or 
minimum control to the cost function to find a minimum time solution that also 
maximizes the efficiency of the system. The modified cost function could take 
the form of Equation (29) where [A] is a diagonal matrix of weighting factors. 
Assuming tf is sufficiently large compared to the running cost, the minimum time 
solution can be found while simultaneously maximizing the efficiency of the 
solution. 

J = +ju^[A]u (29) 

0 

The results of rerunning the same endpoint conditions using Equation (29) 
as the cost function with unity weighting are presented in Figure 21. A number of 
conclusions can be made from this plot. First, it illustrates that there are multiple 
minimum time paths, particularly for those joints that do not limit the minimum 
time. The final time using the new cost function is the same to within 10'^ second 
while decreasing quadratic control cost by 26.3%. This improvement factor was 
found by numerically integrating the calculated control vector of the two 
scenarios. In addition, these results demonstrate the effect of gravity on the 
system. In Figure 20, arm B’s second and third links remain nearly parallel as 62 
and 63 remain at nearly 0 throughout the maneuver. However, including the 
quadratic cost demonstrates that the minimum control torque required to 
complete the maneuver is not the parallel motion. 62 and 63 in Figure 21 actually 
drop below parallel to minimize the total control torque over the course of the 
maneuver. 
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Figure 21. Comparison of Dual Arm Algorithm and Single Arm Algorithms 

Including Minimum Effort 


Changing the cost function, J, to Equation (29), does alter some of the 
necessary conditions, but does not change the two useful conditions used in this 
analysis to check for optimality: H{t)=-^ and A 9 i=constant. What it also does not 
do is simultaneously find minimum time solution for the arm that does not limit 
the minimum time for the system. A viable solution for this using pseudospectral 
methods is not presented here but poses an interesting path for future research. 


D. AN ALTERNATE PROBLEM FORMULATION 

While the standard problem formulation used above finds an optimal 
solution, there are other ways of formulating this optimization problem. One such 
alternate method that was investigated was to reformulate the problem statement 
by eliminating all trigonometric functions. Rather than defining the state variables 
in terms of 0i and coi, let the state variables be defined by the sine and cosine of 
the angles. While this increases the dimension of the state variables, there are 


37 



























no trigonometric functions in the dynamics which may be computationally 
advantageous . The state x, control u and dynamics x for each arm are defined 
for the 2-DOF problem as 












00 

u = 

fr ^ 

X = 

^2^02 

_ 




'K^2j 


-^2^0, 


~^2^02 

co^ 


d). 

' 


«i(x,u) 

y 0)2 J 


[ ; 




Because the states are no longer angles, the following additional path 
constraints are required on the system: 

Cl +Sl -1 = 0 
cl +Sl -1 = 0 

1/2 t/2 

The bounds on the states based on the physical limitations of the system must 
also be reformulated: 


0 < C, < 1 

^1 


2 


- 


<1 


^<c 

- - '^02 - - 


-1<S,,<1 


The endpoints must also be redefined by the new state variables: 


^sin 6 'i ^ 
cos 6 'i 

sin 6'2 

^cos O2 2 


Sf = 


x, (c;,s;,c;,s;) 

y, (c;,s;,c;,s;) 

z, (c;,s;,c;,s;) 


to, =ujf =0 


Using the same endpoint conditions in section 2-A, the alternate problem 
formulation was solved using Equation (29) as the cost function in DIDO. 


38 



Regardless of cost function, tf = 0.92 seconds which corresponds to the minimum 
time solution of the +90° rotation from scenario 1 above. Figure 22 illustrates the 
calculated system trajectory. 



Figure 22. 2 Arm, 2-DOF Simulation Using Alternate Problem Formulation 


This result shows that other proper problem formulations will result in the 
same trajectory as the original problem formulation. Figure 23 compares the 
calculated joint angle trajectories for the two problem formulations. It is apparent 
that regardless of the problem formulation, DIDO computed a very similar 
solution. Flowever, there are preferred formulations for numerical computation 
efficiency. Figure 24 demonstrares that the original problem formulation, for this 
particular model, is better scaled and can be solved an order of magnitude faster. 
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Figure 23. Comparison of Original and Alternate Problem Formulation 
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Figure 24. 30 Node Hamiltonian Values for Different Problem Formulations 
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III. OBSTACLE AVOIDANCE 


A. BACKGROUND 

Of major importance in robotic trajectory planning is solving the obstacle 
avoidance problem. In the most simplistic terms, the issue is one of determining 
the distance between each arm and any potential obstacle. A number of studies 
have been published on obstacle avoidance in motion planning of robotic 
vehicles [1, 12, 28 ,39, 40] where the obstacle is mathematically modeled as an 
enclosed polygon with the vehicle as a point in space and time. A path constraint 
that restricts the point from entering the polygon by a sufficient buffer ensures 
collision avoidance is included in the algorithm. While this is useful in a number 
of autonomous motion planning problems, it does not account for a set of 
continuous points such as arms of finite length. A different path constraint 
formulation is required. 

In the most general sense, each manipulator link and obstacle can be 
modeled as some rigid geometric shape and the minimum distance between 
each link and each potential collision must be calculated at each point in time. 
Solving the obstacle avoidance problem for a continuous set of points rather than 
a discrete point in space and time complicates the problem. In the simplest case, 
each link of each arm can be modeled as a line segment. The minimum distance 
between every two links that have the potential to collide must be computed at 
each time and incorporated into the path constraint. The optimal control problem 
increases in complexity, but pseudospectral methods can still be used to solve 
the trajectory with this path constraint and a given minimum distance between 
links. 


1. Minimum Distance between Two Continuous Lines 

Before discussing the obstacle avoidance algorithm, it is necessary to first 
determine the minimum distance between two static arms. Each link is modeled 
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as a line segment using the parametric equations of a line inD Define and 
as the two endpoints of an individual line segment: 




These values are then used to define the following values for lines a and b: 

ao=Pl a = P"-Pl 

b.=Pl and = 


The equations for two links modeled as a line segment where t and s are 
parametric variables take the form: 


a = ao+af te[0,1] 
b = bo+bs se[0,1] 


(30) 


For any two values for t and s, the distance between the vectors a and b is the 
norm of the vector difference between them. 


c/ = 


a-b 


[K+aO- 


■ (bo + [(ao + at) - (bo + bs)]]^ 


(31) 


It is more convenient to calculate the square of the distance. Squaring Equation 
(31) and grouping terms yields: 

= a^af^ + 2(a^ao - a^^b ^)t - 2a^bsf + • • • 

(32) 

•••+ 2(b’’bo -b^aojs+ b^bs^ +80^0 -2aobo +bobo 
The coefficients of Equation (32) are scalar values. Let 
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yA = a^a 

B = 2(aX-a^bo) 

C = 2a^b 

D = 2(b^bo-bX) 

E = b^b 

F — 3q3q — 2.Bq bg + bg bg 


(33) 


Substituting Equation set (33) into (32) gives 

+Bt-Cst + Ds + Es^+F (34) 

Equation (34) is a parabaloid in □ ^with A > 0 and E > 0- The minimum 
distance for two infinite lines is found by calculating the minimum distance over 
all points t,S€U . This is an unconstrained optimization problem with the cost 
function Y = d^ and the optimization variable x = [ t, s or 

Minimize Y{t,s)^At^+Bt-Cst + Ds + Es^+F (35) 

The minimum must satisfy both the stationary condition and convexity condition: 

^ -0 — 

dx ex' 


>0 

x=x* 


where x* are the parametric coordinates of the minimum distance between the 
two lines. The stationary condition for Equation (35) is 


ex 


er' 


2At + B-Cs ' 


dt 

> — < 


> — < 

dY 


-Ct + D + 2Es 


. ds , 





o' 

0 




and solving for x* yields 


2AD + BC ^._2BE + CD 

(C' -4AE) ~ (C' -4AE) 


(36) 


(37) 
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Likewise, the convexity condition takes the form 


' d^Y d^Y 

d^Y _ ~W di^ 

dx^ d^Y d^Y 

_dsdt ds^ 

Equation (38) is semi-positive definite and meets the convexity condition 
when 4AE- >0. This is true for all t and s and can be seen by breaking it into 
the vector components: 

4AE-C^ =4(a^a)(b^b)-4(a^b)(a^b) 

= 4 |a|^|b|^-(|a||b|cos6')^ >0 

It is interesting to note that a unique solution only exists when 4AE- C^^O. 
When 4AE-C^=0, the two arms are parallel and there is an infinite set of 
solutions. Substituting Equation (37) into (35) gives the minimum distance 
between two infinite lines. 

2. Minimum Distance between Two Line Segments 

While the discussion on the minimum distance between infinite lines is 
useful to this problem, the minimum distance between two arm segments is a 
box-constrained optimization problem: 

miny(f,s) = Af^ + Bt-Cst + Ds + Es^ +F 
subj to 0 < t < 1 (39) 

0 <s<1 

The Lagrangian of Equation (39) is now defined as 

Y(x, A) = AE + Bt - Cst + Ds + Es^ + F + T^t+T^s 
and the first order necessary condition is 
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BY 

— < 

ay' 

dt 

> — < 

2At + B-Cs + Af 

> — < 

O' 

dx 

min 

BY 

.Bs ^ 


-Ct + D + 2Es + T 

.. S ^ 


0 


(40) 


This is a more complicated problem to solve. The parametric coordinates 
for the minimum distance are now functions of the Lagrange multipliers At and As. 

, 2EB + CD + 2ET,+C1 , 2AD + CB + CA,+2AA^ ,,,, 

Imind^ (C2-4^E) (C^-4AE) 


The Lagrange multipliers in Equation (40) and (41) must be found and 
satisfy the Karush-Kuhn-Tucker (KKT) conditions: 




<0 

t = 0 


<0 

s = 0 

= 0 for 

0 <t <^ and 


= 0 for 

0 <s<1 

>0 

t = ^ 


>0 

s = 1 


(42) 


A/ cannot be solved analytically in a general sense and other methods must be 
used to evaluate the endpoints of the segment. 

References [41] and [42] develop geometric algorithms to solve the 
minimum distance between line segments which are summarized below. The 
first step is to calculate 4AE- to find out if the line segments are parallel. Next, 
the minimum parameters t* and s* for two infinite lines that are defined by 
extending the two line segments are calculated. If those values are both 
between 0 and 1, then those are the coordinates of the minimum distance and 
can be used to calculate d Equation (34). If t* is outside the range [0,1] and s* is 
within the range, then the tmin is located at the endpoint closest to t*. Using that 
endpoint condition as a constant. Equation (36) can then be solved for Smin- If 
Smin is now outsido the range [0,1] then s is set to the endpoint closest to Smin and 
a new tmin is computed. The reverse is also true. This approach can be applied if 
the line segments are parallel and if both global minimums are outside of the 
range. 
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Figure 25 demonstrates the algorithm that can be applied to determine 
parametric coordinates that define the minimum distance between two line 
segments using values from Equations (33) and (37). While this algorithm 
efficiently calculates the minimum distance between two static lines, it requires 
nine if-then statements and is not necessarily a continuously differentiable 
function over time. 



Figure 25. Geometric Algorithm for Min. Distance Between Line Segments. 
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B. STATIC OBSTACLE AVOIDANCE 


1. 2-DOF Case 

The above algorithm was used as a path constraint to define the minimum 
distance between the centerline of an arm and the centerline of a cylinder 
defined by the respective endpoints. DIDO allows the path constraint to be a 
function that incorporates the complexity of the geometric analysis. The problem 
formulation is identical to Equation set (1515) except the added path constraint 
is included where d is the instantaneous distance between the arm and 

the obstacle and dmin is the pre-defined obstacle avoidance distance. For the 
purposes of simulation, c/min= 5 cm. The centerline of the obstacle was placed 45 
cm above the base of the arm and oriented along the y axis of the Cartesian 
coordinate system. Once again, an initial 16 node solution was calculated and 
then used as a guess for a more refined 60 node solution in order to interpolate a 
feasible control trajectory when propagating the solution using ODE45 in Matlab. 
The relevant initial and final conditions of the simulation are included in Table 4. 


o 

II 

1 

CT) 

O 

o 

[Xef, yef, Zef] =[.186, .281, .483] 

[X 1 , yi, zi]obs = [-0.5, 0, .55] 

o 

LO 

II 

o 

dm'tn ~ 5 cm 

[X2, yi, Z2]obs =[ 0.5, 0, .55] 


Table 4. Example Endpoint Conditions for Single 2-DOF Arm with Static 

Obstacle Avoidance 


Figure 26a demonstrates the solution for the defined maneuver without 
imposing the obstacle avoidance path constraint and using the cost function 

J = tf + .5ju^u 

0 

As the figure illustrates, the arm passes through the centerline of the 
obstacle. Figure 26b demonstrates the same maneuver with the collision 
avoidance algorithm active. The trajectory of the arm changes to allow a 
minimum 5 cm buffer between the arm and line. Figure 27 -Figure 29 provide the 
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corresponding state and control trajectories, the costate values over time, the 
Hamiltonian value, and the variation of the minimum distance between the arm 
and the obstacle during the obstacle avoidance maneuver. 
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Figure 27. 2-DOF Motion with Static Obstacle Avoidance: State and Control 

Trajectories 
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Figure 28. 2-DOF Motion with Static Obstacle Avoidance: Costate and 

Flamiltonian Trajectories 



Figure 29. 2-DOF Motion with Static Obstacle Avoidance: Distance between 

Link and Obstacle (5 cm Buffer) 
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2 . 


3-DOF Case 


Similar to the 2-DOF case, the 3-DOF problem formulation is identical to 
Equation Set (25) with the added complexity of the path constraint imposed to 
guarantee a minimum distance between both links and the obstacle. The path 
constraint for this problem includes 

<min ^ < 

where c/i min are the minimum distances between the obstacle and each 
respective link. The joint positions must be computed at each node in time using 
the kinematic model and these points define the line segments used to compute 
the minimum distance. 


CD 

o 

II 

CO 

O 

o 

[Xef, yef, Zef] = [.029, -.334, .049] 

[X 1 , yi, zi]obs =[-1,0, 0.4] 

o 

LO 

II 

o 

dlmin “ 0.05 m 

[X2, Yl, Z2]obs =[1,0, 0.4] 

O 

O 

II 

o 

<I> 

d 2 min “ 0.05 m 



Table 5. Example Endpoint Conditions for Single 3-DOF Arm with Static 

Obstacle Avoidance 


Figure 30a demonstrates the minimum time solution for the above 
maneuver without the obstacle avoidance algorithm. As the figure illustrates, the 
arm passes through the centerline of the line used to model the obstacle. Figure 
30b demonstrates the same maneuver with the collision avoidance algorithm 
active. The trajectory of the arm changes to allow a minimum 5 cm buffer 
between the arm and static cylinder. The maneuver time for both simulations 
was 1.22 seconds. The difference in the trajectory can be attributed to the 
weighted quadratic cost function. Figure 31 - Figure 33 are plots of the solution 
trajectories. Of particular interest are the Hamiltonian and Aei that still satisfy the 
necessary condition derived in the original 3-DOF case. 
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[a] [b] 

Figure 30. 3-DOF Maneuver Without and With Static Obstacle Avoidance: 60 

Node Solution 
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Figure 31. 3-DOF Motion with Static Obstacle Avoidance: State and Control 

Trajectories 
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Figure 32. 3-DOF Motion with Static Obstacle Avoidance: Costates and 

Flamiltonian 


Distance Between Link A and Obstacle Distance Between Link B and Obstacle 



Figure 33. 3-DOF Motion with Static Obstacle Avoidance: Distance between 

Links and Obstacle 


In applying the PS based obstacle avoidance algorithm, one must note 
that the numerical solution, uses discrete times or nodes to calculate the state 
and control trajectories and the path constraints are only tested and guaranteed 
at each of these nodes. A low node solution, while feasible at each discrete 
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node, may not to be physically realizable. Figure 34 and Figure 35 illustrate a 
16- node solution for the same problem. The nodal spacing in these cases is 
such that the trajectory seems to jump the obstacle, that is the path constraints 
are met at the two nodes on either side of the obstacle as shown in Figure 35 but 
the arm trajectory would collide with the obstacle as it proceeds from one node to 
the next. The possibility of nodal jumping requires that the computed trajectory 
be analyzed to ensure it is feasible and may necessitate increasing the number 
of nodes, and therefore, the computational time to find a feasible solution. 



Figure 34. 3-DOF Motion with Static Obstacle Avoidance: 16 Node Solution 


Distance Between Link A and Obstacle Distance Between Link B and Obstacle 



Figure 35. Minimum Distance Between Links and Obstacle,16 Node Solution 
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c. 


NUMERICALLY SOLVING THE KKT CONDITIONS 


Returning to the discussion of minimum distance between line segments, 
solving Equation (41) should produce the minimum distance parametric 
coordinates. While this equation cannot be solved analytically, it can be solved 
numerically using DIDO. 

Taking a step back to basic optimality, the constraints on the variables 
can be rewritten in the form y(x) < 0 where y is a vector of constraint functions. 

minimize Y{t,s) = At^ +Bt-Cst + Ds + Es^ +F 
subj to -t<0 -s<0 

f-1<0 s-1<0 

Taking the Lagrangian of the cost function yields a new optimization 
problem. 

minimize Y{x,K) = AF + Bt-Cst + Ds + Es^ + F - + Af 2 {t- + 252 ( 5 - 1 ) 

subj to A > 0 

A^y = 0 


Rewriting Equation (40) using the new Y gives 


dY 


dx 


Imin 


dY 

r 

dt 

> = < 

dY 


. ds. 



2At + B - Cs - 
—Ct + D + 2Es — A^.^ 


+ A 


(2 


+ 2 , 


's2 


with the constraints 


^(-0=0 4i(-5) = o 

4(f-i)=o 42 ( 5 - 1 ) = o 


fol 


0 


(43) 


(44) 


By letting s, t, and X-, be six dummy control variables for each possible 
collision in the problem formulation, DIDO can numerically solve the problem with 
the KKT conditions that resemble the distance between two lines. Defining the 
minimum distance allowed between two arms. Equations (34), (43) and (44) 
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specify seven path constraints that are associated with each potential collision. 
While this increases the number of variables and functions, it directly solves the 
original obstacle avoidance problem without geometric checks and resulting 
pitfalls described by Figure 25. 


1. 2-DOF Case 

The motion planning problem was reformulated to include the dummy 
control variables and additional path constraints for the case of two 2-DOF arms: 


u 


=. 


^2 , T’o , ^1 , ^*2 J 


, /l^2 j ^s1 ’ ^s2 


h" <h[x(D),u(D)]<h" 
c/^in < At^ +Bt- Cts + Ds + Es^ + F < 00 
0 ^ 2.At + 6 — Cs — + A,f 2 — 0 
0 ^ —Cf + D + 2.Es — — 0 

0 < (-f H, < 0 
o<(f-iK2 <0 
0 < (-s)4, < 0 
0<(s-1K2<0 


Using DIDO, the optimal trajectory was computed by solving the distance 
between the arms at each node and bounding the dummy control variables: 

0 <t <1 

0<s <1 

0 < T, < 00 

The results of this formulation are compared to those found using the 
geometric algorithm and presented in Figure 36 and Figure 37. The state and 
control trajectories are nearly identical with tf and cost differences are on the 
order of 10'®. The only significant difference was the computational time. On 
average, the KKT algorithm converged to a solution for the 2-DOF problems 
nearly twice as fast as the geometric algorithm. 
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Figure 36. 2-DOF Static Obstacle Avoidance: State and Control Trajectories 
(KKT Algorithm versus Geometric Algorithm) 
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Figure 37. 2-DOF Static Obstacle Avoidance: Flamiltonian Values (KKT 
Algorithm versus Geometric Algorithm) 
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3-DOF Case 


The 3-DOF example was also reformulated using the KKT algorithm with 
an 18-element control vector, u and 14 path constraint equations. Figure 38 
presents the results of this simulation. The maneuver time for both the geometric 
algorithm and the KKT algorithm are the same to within 10'^ at 1.221 seconds. 
Both functions used a cost function of the form 

t, 

J = ff+.lju^u (45) 

0 

The difference in cost between the two algorithms was 0.001 and may 
account for the slight difference in 1:2 and 1:3 trajectories. The Hamiltonian 
presented in Figure 39 corresponds to the required necessary condition, H=-^, 
just as it did for the geometric algorithm. The distance between the obstacle and 
the two links of the arm are presented in Figure 40. 

The only major difference between the solutions for the two minimum 
distance algorithms is the computational time. It is difficult to accurately assess 
the runtime using DIDO because other processes are often running on the 
computer and solution times vary considerably for the same problem. Typically 
runtimes for a 16-node solution took 5-15 minutes regardless of the algorithm 
used. In an effort to make a meaningful comparison, a previous 60 node solution 
was used as an initial guess for each problem formulation. A 60 node solution 
took 115 seconds to run for the KKT algorithm compared to 93 seconds for the 
geometric algorithm. The trend appears that as the complexity of the problem 
increases and the dimension of control vector increases, the KKT solution is 
slower than the Geometric solution for the same problem. 
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Figure 38. 3-DOF Static Obstacle Avoidance: State and Control Trajectories 
(KKT Algorithm versus Geometric Algorithm) 


O 

CD 

> 

C 

TO 

‘c 

E 

CD 

I 




I 

1 

1 

1 1 ~ 

- KKT Algorithm 



1 

1 



— 

1 1 

1 1 

1 1 

1 1 

1 1 

1 1 

1 1 

M - 

__ 

_ 

1 1 

1 1 

1 1 

1 1 

1 1 

_I_^_ 

_ 


0 0.2 0.4 0.6 0.8 1 1.2 1.4 


Time [sec] 


Figure 39. 3-DOF Static Obstacle Avoidance: Flamiltonian Values (KKT 
Algorithm versus Geometric Algorithm) 
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Figure 40. Minimum Distance Between Links and Obstacle (KKT Algorithm 

versus Geometric Algorithm) 
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IV. COOPERATIVE PLANNING USING PS METHODS 


The implementation of the obstacle avoidance algorithm in Chapter III 
demonstrates that pseudospectral techniques are capable of solving a range of 
problems. While the model was specific to an arm and a static cylinder-type 
object, the next step is to implement the same concept to find an optimal path to 
more complex systems. As part of this research, one interesting problem is to 
solve the optimal control trajectory of a multiple arm system where the arm links 
are modeled as line segments. Path planning for two arms has already been 
presented in Chapter II. Adding the path constraints from the obstacle avoidance 
algorithms presented in Chapter III allows for cooperative or, at the least, 
simultaneous control of a system of arms. Two examples are presented below 
using the dual arms that detail optimal path planning for 2-DOF and 3-DOF 
motions. 

A. DUAL 2-DOF MANIPULATORS 

The dual 2-DOF arm cooperative planning problem is a straight forward 
modification of the static obstacle formulation. The obstacle is now each link of 
the other arm. The minimum distance between the links of the two arms can be 
computed for any given time using either of the algorithms presented in Chapter 
III and can be constrained to be greater than some preset distance. Table 6 
presents the endpoint conditions used to define the problem. 


Arm a 

Arm b 

o o 

CD CD 

II II 

o o 

xf = 0.337 
yf =-0.02 
zf = 0.483 

6'“ = 0° 

6'f = 45° 

xf =0.102 
yf = 0.084 
zf = 0.559 


Table 6. 2-DOF Cooperative Path Planning Endpoint Conditions 
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The two arms were offset by 0.30 m along the y axis and the final 
coordinates of the end-effector were chosen based on angles 0i = 0° and 02= 45° 
for Arm a and 0i = -60° and 02 = 60° for Arm b. DIDO was used to solve the 
problem with thecost function from Equation (45). The path constraint uses the 
endpoints of the arm links at each “node” to compute the distance between the 
arms and was constrained to be above some minimum distance c/min- 

The optimal maneuver for this particular setup with no obstacle avoidance 
algorithm dm\n= 0 has a closest point of approach of 10 cm and takes 0.75 
seconds to complete. Figure 41 shows such unconstrained optimal state 
trajectory and Figure 42 is the minimum distance between the arms throughout 
the maneuver. For this maneuver, J=0.968. 

Arm a Arm b 






Figure 41. 2-DOF Cooperative Path Planning with No Obstacle Avoidance: 

State Trajectory 
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Figure 42. 2-DOF Cooperative Path Planning with No Obstacle Avoidance: 

Distance between Arms 



Figure 43. Cooperative Path Planning for Dual 2-DOF Arms with 19.5 cm 

Minimum Clearance 
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If 10 cm is not sufficient for the arms to avoid colliding it is imperative to 
modify the problem and define a more restrictive c/min- For the maneuver with the 
above endpoint conditions, the maximum buffer distance between the arm links 
is 19.7 cm. The following example uses the same endpoint conditions; however, 
c/min was given as 19.5 cm. DIDO was run using both the geometric and KKT 
collision avoidance algorithms with similar results. Figure 43 depicts the arms at 
four points along the computed path. A red line is attached to the points on each 
arm where the minimum distance between the arms occur. While not clear in the 
plots, that minimum distance line is perpendicular to both arms. Figure 44 - 
Figure 47 show results using the geometric algorithm for obstacle avoidance. 

Arm 1 Arm 2 






Figure 44. 2-DOF Cooperative Path Planning using Geometric Algorithm and 

19.5 cm Buffer: State Trajectories 
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Figure 45. 2-DOF Cooperative Path Planning using Geometric Algorithm and 

19.5 cm Buffer: Control Trajectories 




Figure 46. 2-DOF Cooperative Path Planning using Geometric Algorithm and 

19.5 cm Buffer: Flamiltonian Value 



Figure 47. 2-DOF Cooperative Path Planning using Geometric Algorithm and 
19.5 cm Buffer: Minimum Distance between Arms 
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Compared to the case with no obstacle avoidance, the total cost of the 
dynamic optimization problem for the optimal path maneuver including a 19.5 cm 
buffer was found to be J=1.067, a 10% increase. The maneuver time also 
increased to 0.82 seconds and the weighted quadratic cost increased by .02 
units. The only substantial difference between the KKT algorithm and the 
geometric algorithm was the computational time to converge to a solution. On 
average, the KKT formulation was greater than twice as fast as the geometric 
algorithm. Using a 60-node solution with a previously computed 60-node guess, 
the KKT algorithm took 11.9 seconds to solve versus 35 seconds for the 
geometric algorithm. 

B. DUAL 3-DOF MANIPULATORS 

The multiple 3-DOF robotic arm obstacle avoidance problems are more 
complicated. The multiple time-optimal paths for each arm plus the nearly infinite 
obstacle avoidance paths make this an interesting, but challenging problem. 
While the algorithms presented to this point are theoretically capable of solving 
the problem, the number of potential collisions create more complex path 
functions. For this 3-DOF, dual arm system, there are six potential collisions that 
must be accounted for vice one in the relatively simple 2-DOF system above. In 
the case of the KKT algorithm, this results in 42 path constraints and 36 “dummy” 
control variables that compute the distance between any two links. Table 7 
summarizes the endpoint constraints used for this simulation. 


Arm A 

Arm B 

LO 

1 

II 

o 

xf =0.435 

= 90° 

xf = 0.435 

LO 

II 

o 

yf = -0.030 

LO 

II 

o 

yf = -0.330 

o 

II 

cn 

zf =0.270 

o 

II 

o 

zf = 0.270 


Table 7. 3-DOF Cooperative Path Planning Endpoint Conditions 
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For this example, 0l = 0° was used to compute the final endpoint position 

in Table 7 for both arms to ensure the feasibility of the final condition. Arm B was 
placed 30 cm from Arm A in the -y direction with the same 3-DOF limits from 
Equation set (25). As was presented in the 2-DOF case, Figure 48 and Figure 
49 present the results with c/min=0. The closest approach occurred between the 
manipulator links of the two arms (Link 3 in Figure 49) when they come within 1.6 
cm. 


Arm A Arm B 





Figure 48. 3-DOF Path Planning without Obstacle Avoidance: State 

Trajectories 
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Arm A Link 1 & Arm B Link 3 Arm A Link 3 & Arm B Link 1 





Figure 49. 3-DOF Path Planning without Obstacle Avoidance: Distance 

between Links 

One of the advantages of using DIDO is the ease in which the problem 
can be designed. For example, the buffer distance can be tailored for each 
potential collision based on the physical dimensions of each link. In this case, 
each link was assumed to be the same width and c/min= 8 cm for all six potential 
collisions. DIDO was run using an initial three-point guess for a 16-node solution 
and then a bootstrap approach for subsequent iterations. The middle point of 
this guess was nominally chosen so that both arms would orient vertically with 0i 
at the midpoint the same as 0io. Intuitively, raising the arms to vertical before the 
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base turns would create separation and provide the arms with an obstacle free 
path in most cases. In general, this was found to reduce the computation time. 
Figure 50 sketches the 60-node optimal path of both arms computed by DIDO 
with an dmin = 8 cm. The obstacle avoidance maneuver took the same time as 
the the case with dmin = 0 cm, 10'® seconds, with an increase in the quadratic cost 
of 3.5%. Figure 51 -Figure 53 display the state and control trajectories, costate 
values, and Flamiltonian values computed by DIDO. 





Figure 50. Cooperative Path Planning for Dual 3-DOF Arms with 8 cm Buffer 
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Figure 51. 3-DOF Cooperative Path Planning using Geometric Algorithm and 

8 cm Buffer: State Trajectories 


Arm A Arm B 



Figure 52. 3-DOF Cooperative Path Planning using Geometric Algorithm and 

8 cm Buffer: Control Trajectories 
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Figure 53. 3-DOF Cooperative Path Planning using Geometric Algorithm and 
8 cm Buffer; Costate and Flamiltonian Values 


While the Flamiltonian value is constant at -1 from Figure 53, the value for 
Xei for Arm A does not appear to be constant and steps up at the midpoint of the 
maneuver. Changing the scale of the chart and plotting the Arm B values on 
top of the Arm A Xei shows that this step up is of the same order of magnitude as 
the oscillations in the other costate values due to numerical computation errors 
seen in Figure 54. Figure 55 illustrates that the avoidance algorithm does 
provide the required buffer of 8 cm between the links. 
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Figure 54. 3-DOF Cooperative Path Planning using Geometric Algorithm and 

8 cm Buffer: >.01 Plot 

Arm A Link 1 & Arm B Link 3 Arm A Link 3 & Arm B Link 1 








Figure 55. 3-DOF Cooperative Path Planning using Geometric Algorithm and 

8 cm Buffer: Distance between Arms 
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To ensure feasibility of the solution, the DIDO computed control trajectory 
was linearly interpolated and used to propagate the system using Matlab’s 
ODE45 function and a fixed time step of 0.05 seconds. The results of the 
propagated states, shown in Figure 56, verify the feasibility of the control 
solution. The KKT algorithm was also run using the endpoint constraints from 
Table 7 with nearly identical results to the geometric algorithm. Figure 57 
compares the two state trajectories and Figure 58 plots the costate values that 
correspond well with the geometric algorithm shown in Figure 53. 

Arm A Arm B 




Figure 56. 3-DOF Cooperative Path Planning using Geometric Algorithm and 
8 cm Buffer: Propagated State Values (o) compared with DIDO values (-) 
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Figure 57. 3-DOF Cooperative Path Planning: State Trajectory (KKT Algorithm 

versus Geometric Algorithm) 



-0.02 



Time [sec] 


Arm B 




Figure 58. 3-DOF Cooperative Path Planning using KKT Algorithm: Costate 

Values 
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While the solution paths between the KKT and geometric algorithms are 
similar, two differences are worth pointing out. Using the KKT algorithm, DIDO 
computed a cost function that was 7% lower than when it used the geometric 
algorithm. Since the final maneuver time was the same to 10'® seconds, the 
difference corresponds to a 21% decrease in the quadratic cost. While the KKT 
algorithm seems to converge to a better cost function, a computational price is 
paid. On average DIDO took three times longer to converge to a solution using 
the KKT algorithm than using the geometric algorithm. The fact that the KKT 
algorithm converges to a slightly lower cost solution infers that the geometric 
algorithm may not be as accurate for complex systems and the KKT algorithm 
may be more robust. 

Whether this computational penalty can be alleviated is an interesting path 
for future work. In general, with the 3-DOF problems visited in this work, the 
costate values were an order of magnitude smaller than the corresponding state 
values. This brief comparison may indicate that the problem can be better scaled 
and balanced using some unknown designer units, which would allow the 
pseudospectral method employed by DIDO to converge faster to a more 
accurate optimal solution. 
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V. CONCLUSION AND RECOMMENDATIONS 


Application of pseudospectral methods for motion planning of multiple 
mulit-DOF robotic manipulators was studied. The use of DIDO for obtaining 
optimal trajectories allowed the focus of effort to be placed on the problem 
formulation instead of solving the optimal control problem. Rather than starting 
with a complex problem that incorporates trajectory planning and obstacle 
avoidance, a staged approach was found to be an effective means of developing 
the final optimal control problem formulation. First, a relatively simple 2-DOF 
problem was formulated and solved with all the necessary conditions for 
optimality derived and verified. Building on this basic problem formulation, the 
complexity of the system was increased to 3-DOF and then with the addition of a 
second robotic manipulator a 6-DOF system. Pseudospectral techniques were 
effective in quickly solving optimal pick-and-place paths. While not trivial, the 
addition of higher DOF arms and increasing the number of arms can be 
accomplished relatively simply within the pseudospectral framework. 

The choice of cost function is an important element that has a major effect 
on the ultimate solution. While a minimum time trajectory is desired, some level 
of efficiency should be included, particularly when dealing with decoupled 
elements, or even lightly coupled elements as is often the case with multiple links 
and arms. A weighted cost function that includes both a minimum time element 
and a measure of minimizing effort is desirable. Once again, the use of DIDO 
allows the problem formulation to be flexible and easily accommodate a wide 
range of cost functions based on the mission objectives. 

Even the relatively simple model of a robotic arm using rigid links and 
perfect joints had a fairly complex form. A higher fidelity model that includes joint 
parameters and system flexibility can be substituted in for the dynamics of the 
system. While the level of effort to model the system would surely increase, the 
optimal control framework may exploit the more complex interactions to find a 

solution that increases the performance of the system. 
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Obstacle avoidance was included by defining path constraints that 
consisted of the minimum distance between the manipulators’ links. Using 
parametric equations to define each link, an optimization problem was formulated 
and simultaneously solved to determine the minimum distance between each link 
and any potential obstacle. Two techniques are presented that take advantage 
of the flexibility of problem formulation in DIDO. The results suggest that for 
higher order systems, the geometric function for finding the minimum distance 
converges faster when provided a feasible guess. The algorithm based on 
numerically solving the KKT conditions seems to be better behaved and solves a 
low-node problem with a simple two-point guess more reliably. As the complexity 
of the system increases, the robustness of the KKT algorithm can be used to 
solve a low node solution, which can then be used as a guess for the geometric 
algorithm to refine the trajectory with less computation time. The refined control 
solution can then be interpolated and used for a physical implementation. 

As implemented, the use of the proposed obstacle avoidance techniques 
requires knowledge of all obstacles in the workspace. It also assumes that each 
link in the system is modeled as a line, which may not be ideal for some systems. 
Further efforts can be placed into modifying the parametric distance function for 
complex shapes. 

By not having to focus on an analytical solution to the optimal control 
problem, variations in problem formulation can be explored. Being able to 
compare the results of different problem formulations may result in the discovery 
of computational efficiencies. Here, no scaling was done on any variables. By 
experimenting with designer units, the computation time to solve the problem can 
be improved with no change in the physical trajectory. Another variation not 
considered in this study is the use of the links’ minimum distance function in the 
cost as a pseudo-repulsive force or penalty function. 

While the robotic manipulators studied here are relatively simple, the 
techniques presented can be used to solve more complicated problems that just 
a few years ago were considered unsuitable for real-time use [5]. Collision 
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avoidance and trajectory planning for cooperating arms using pseudospectral 
methods can also be studied for real-time autonomous implementation provided 
computational efficiencies are exploited. 
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APPENDIX - 2-DOF ARM DYNAMICS COMPUTATION 


Step 1: Coustmct the DH transfonuation matirx and its inverse... 


> H := Matrix( [ [cos(0).-cos(a) ■sin(9). sin(a) ■sin(0). (7 cos(0) ]. [siii(0). cos(a) 
•cos(0).-siii(a) •cos{0), rt' siu(0) ]. [O. sin(a). cos(a), d ]. [0. 0. 0. 1 ]]); 

Hinv ■— Matrix^ [ [cos{0). sin( 0), 0,-rt']. [ -cos(a) * sin(0). cos(a) * cos( 0), siii(a) .-rt' 


* siii(a) ]. [siii(a) * siii(0).-siii(a) * cos(0). cos(a).-^/*cos(a) 
cos(0) -cos(a) siii(0) sin(a) sin(0) rt'cos(0) 

sin(0) cos(a) cos(0) -sin(a) cos(0) <7siu(0) 


H = 


sin (a) 
0 


cos(a) 

0 


d 

1 


[ 0 . 0 . 0 . 1 ]]); 


Him-x 


cos(0) sin(0) 0 -o 

-cos(a) sin(0) cos(a) cos(0) sin(a) -<ysin(a) 
sin(a) sin(0) -sin(a) cos(0) cos(a) -/7cos(a) 
0 0 0 1 

Step 2: Construct the link frame transfonuation matrices... 


Rule for D-H transformation: 

1. translate along the z_(i-l) axis a distance d_i 

2. rotate about z_(i-l) axis an angle theta_i to bring x_i axis into alignment with x_(i-l) axis 

3. translate along x_i axis a distance a_i 

4. rotate about x_i axis an angle alpha ! 


D-H Table: 


link alpha a d theta 


1 

2 


+Pi/2 al dl theta 1 
0 a2 d2 theta 2 


> // := 2 : 

> HOI ■= iiiap^x^ey(7/^s//bs^^ =-\- rt" = rt'j, d = d^, 0 = 0^, .r j j, j; 

IIJ2 ■— mapera/^ s//bs(^ci. = 0, a = a^ d = dy 0 = 0.^ .r^ ^; 


* Courtesy of Dr. Mark Karpenko, Guidance and Control Laboratory, 
Naval Post Graduate School, Monterey, CA. 
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cos^GJ 

G sin(Gj) 

(7j COS^Gj^ 

H01\= 

sin(Gj) 

G -cos^gJ 

77i sin(G,) 


G 

1 

G 



G 

G 

G 

1 


cos(G,j 

-sin^G^j G 

77, COS^G-,^ 

H12~ 

sin ^ G, ^ 

cos^G^) G 

77, sin^G,^ 


G 

0 

1 



G 

0 

G 

1 




Step 3: forward transformation matrices 


> HO2 — Muhiph ■ ( HOI. HI 2 ); 

H02'- Jl^cos^Gjj cos^0,j. -cos^Gj^ sin^G^). sin^Gj^. cos^G^ j rt-^cos^ 

+ (7j cos^GJ |. 

l^sin^Gj) cos^G-,^. -sin^Gj^ sin^G,^. -cos^Gj). sin^Gj^ ^/^cos^G,^ 
+ rt'j sin^Gj j j, 

l^sin^G,^. cos^G,^. G. <7^ sin^G,^ + t/jJ. 

[G. G. G. ij] 


G^) +sin(Gj) d^_ 
— cos^Gj^ d^ 


> q2x -.= H02^ ^. 
q2y := H02^ 
cj2=-.= H02^_^. 


q2x \= cos^ 0T) + ®i) 

^_’r;= sin^Gj^ ^r^cos^G^^ — cos^Gj^ + 77^ sin^Gj^ 
q2z := 77, sin ^ G, ^ 


> CodeGeneran'onyMadab''\[q2x) : 

eg = cos(theta(1)) * a(2) * cos(theta(2)) + sin(theta(1)) * d 
Ji2) + a(l) * cos(theta(l) ) ; 

> CodeGeneratiofi [ 'Madab' ] ( q2y ■) 

egO = sin(theta(1)) * a(2) * cos(theta(2)) - cos(theta(1)) * d 
^(2) + a(l) * sin(theta(1)); 

> CodeGeneradon [ 'Madab' ] ( q2z ) 

_cgl = a(2) * sin(theta(2)) + d(l); 


Step 4: Link Jacobians 
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[> 

Rotation matrices 
> ROO : = Matrix (3); 

ROl ■■= SubMatrix^HOL [ 1 ..3], [ 1 ..3]); 
R02 SubMatrix\H02, [ 1 ..3 ]. [ 1 ..3 ]); 

R00 = 


1 

0 

0 


0 0 
1 0 
0 1 


R02 ■- 



cos^Gj j 

0 

sin^Gj^ 

R01 = 

sin(Gj) 

0 

-cos^Gj^ 


G 

1 

G 


cos 


cos 


-cos(Gi 

sin 

(«4 

sin^Gj j 

sin| 


cos| 


-sin(Gi; 

sin( 


-cos^Gjj 


sin 



COS| 



0 


Position matrices 

“> rOl ■- S//bMatrix{HOR [ 1 ..3 ], [4]); 
rl2 ■■= SiibMatrix\H12 [1 ..3]. [4]); 

rOJ := 


a^ sin^Gj j 


fJ2:= 


cos^G^j 
<7-, sin^G,j 




Z-vectors 

> - 3 ^^== 1 ])’ 

:0 — Muhiplyi^ ROO, 
zl ■- Miiltiplyi^ROl, 


base' 


G 

G 

1 
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: 0 .= 


-/:= 


-ink mass-centers 

[A;]. [/-]]); 

P22 - .Vamx( I [A,]. [A;]. li-jJJ); 


sm 

-cos 

0 


(«.) 

i\) 


Pll = 


P22- 


Iz. 


Transformed mass-centers 
> Pllsrar := Multipfy{R01, Pll), 

POlsrar ■■= Multipl}\ROO, rOl) -h Pllsrar, 

cos(ei)/-*-, +sm(ej)/L- 
Pllstar := sm( Gj) Zi^ - cos( Gj) 


POlstar ;= 


cos(Gj) -I- cos(Gj^ ZTj -I- ZTj 

^7j sin^Gj^ -I- sm(Gj) Zij — cos(Gj) Iz^ 


d, + 


> P22star ■= MultipIy{R02 P22\, 

P12srar ■■= combme(MuIriph'(ROJ, rJ2 )) -I- P22star, 
P02star ■■= comd/ae(Mulriph'{ROO, rOJ )) -I- PI2star \ 
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P22srar:= 


cos(0j) cos(0,) Zt, — cos^0j) sin(0,) fy\ + iin(0j) 
sm(0j) cos(0,) ZVj - sm(0j j sm(0,) - cos(0j) 

sm(0,) iK, + cos(0,) 


P12star.= ^-,cos(0j — 0,^ + y ^7,cos(0j + 0,) + sm(0j) cos^0j^ cos(0,j /c, 

— cos(0j) sm(02) + sm(0j) Zr, 

y sm(0j + 0,J + y sm(0j — 0,) — cos(0j) d^ + cos(0t) bL, 

- sin(0j j sm(0,) fy\ — cos(0j) Zr, 

(7, sin^0,^ + sm^0,) lx, + cos^0,) A; 


3uild the Link Jacobians 

> zeros != Transpose^ Vector^ (0, 0,0, 0, 0,01)): 

Jill ■■= Trampose{ Vector{ \ zOSix Vector{ [ P0Jstar\ ),z0])): 
JI21 ■■= zeros'. 

JIl ■■= combine^ ( rranspose{ Matrix^ \ \ Mil ], [ JI21 ]])))); 

-^7j sm^0jj — sm|0j) ZVj + cos(0j j Iz.^ 0 




<7j cos(0j) + cos(0j) ZTj + sm( 0j) ZTj 0 

0 0 

0 0 

0 0 

1 0 

> JI12 ■•= Transpose^ Vector( [zOSix Vector(\P02star \), zO])): 

JI22■■= Tra/ispose{ l'ecror( Fee/or([P/2s/arj ), z/])): 

JI2 ■■= combine^ Transpose^ Marrix^ [ [ JIJ2\ [ JI22\ ]))); 

^1 + ®2) “ T “ %) < 

y Zi^ sm(0j + e,) - y be, sm(0j - e,) + y dj cos(0j “ 0,) “ y ^2 


212 = 


1 , 

1 / 

1 , 


y ^ 7 , sm( 0 j + 0 ,) 

+ y ^ 7 ,Sm( 0 j- 0 ,) 

-y^ 

j 2 sin( 0 , + 0 ,) 


+ y Zi;sin(0j - 0 ,) - 1 4;cos(0j - 0 ,) - 1 A’cos(0j + 0,) j. 
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cos(0j) + y cos(0j — 0,) + y < 7 , cos(0j + 0,) + sm(0jj </, + y Zc, cos(0j 

— 0,) + y Zl^ cos(0j + 0,) - y Zj^ sm(0j + 0,) + y sm(0j - 0,) 

+ sm(0j) Zr,, - y ^7, “ ®;) + y ^2 cos(0i + ®;) “ y cos(®i ~ ^ 2 ) 

+ y Zt, cos(0, + 0,) - y A', sm(0j + 0,) - y sm(0j - 0,) j, 

0, cos(0,) + cos(0,) Z^ - sin(0,) Aj , 

[o.sin( 0 ,)], 

[o,-cos(0,)], 

-ink Inenia \Iatnces 

> Ill ■■= Matrix^ \ \ Loci, Ix\l, Ixzl ], ( Axl, ^rl. Ml 1, [ Lxl, Izy 1, Izzl ) ]): 

J1 •■= Miilnpfy{R01,Mulnpl}\lll, frampose{R01 '\)); 

^ |cos^0jj ^Ai7cos(0jJ +/i2'/sm(0j^J +sm^0j^ ^A3'/cos^0jj + 7^/sm^0jJ j, 
cos(0jj ^ATZsm(0j) — Arycos^0j)) + sin^0j) ^Aiysm^0jj — Z”ycos(0j)), 
cos(0j^ Ixyl+ sm(0jj 

Jsm^0j) ^Aiycos(0j) +/crysm(0j)) — cos(0j) (Aiycos|0jj + Z”ysin(0j)), 
sin(0j^ ^yuysin^0j — /crycos^0j^ ^ — cos^0jj ^Aiysm^0j^ — Arycos(0jJ j, 
sin (0j J hyl — cos( 0j) 1^1^ 

jATycos(0j^ + Arysm^0j), ATysm(0j^ — A2’ycos(0j), AjiV] | 

> 122 ■■= Matrix^ [ [ Lcx2, Ixy2, Ixz2\, ( ^x2, Ay2, M2], [ 12x2, lzy2, Jzz2 ) ]): 

I2-= Mulnpl\\R02,Mulnpl}\122, franspose(R02 ))); 

Z?:= [|cos(0jj cos^0,) ^Aiycos(0j cos^0,) — /n-'’cos^0jj + ■^■-i’sin(0j^) 

— cos(0j) sin(0,) (yjrJcos^0j) cos(0,) — wArycos(0j) sm(0^) +Arysin(0j)) 

+ sm(0j) (Ai.?cos^0j) cos^0,^ — Zi’ycos(0jJ sm(0,^ + Arysm(0jJ 
cos(0j^ cos(0,) (/trysin(0j) cos^0Tj — i^'-?sm(0j) sin^0T) — Azycos(0j)) 

— cos^0j) sm(0,J (j5^J'sin^0j^ cos(0,^ — ATysin(0j) sm(0,) — A2ycos(0jJ) 

+ sm(0j) (Aii^sin(0jj cos^0,) — yn.ysin^0jj sin^0T) — Azycos(0j)J, 
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cos(0jj cos^St) (ArJsm^0Tj + i^L^cos|0Tj^ — cos^0jj sm|0Tj (ATJ’sin(0,) 

+ /n'J’cos|0,j j +sm^0j| ^Ari’sm^0,^ + Z7J2’cos(0,)) j, 

[sin^0j) cos(0t) (AtZcos(0j) cos^^Q^'j — AZcos(0j| sm|0Tj + Zzi’sin(0j)) 

— sm(0j^ sin(0,) (^a'Zcos^0j^ cos|0,J — ZtZcos^0jJ sin(0,J + ^2i’sm(0j) J 

— cos(0j) ^Z:rZcos(0j) co%(^e^'j — Z7jZcos(0jj sin(0,) +ZzJ’sin(0j 
sm^0j^ cos^0,^ ^ATi’sm^0j^ cos^0,J — ^Zsin^0j^ sin^0,^ — ArZcos(0j^^ 

— sm(0jj sm(0,) (^TZsm^0j) cos(0,J — AT'Zsm^0jj sm^0Tj — Azi’cos^ 0j j J 

— cos^0j) ^ArZsin^0jj cos^0,^ — AjZsm|0j^ sm^0,^ — Z“Zcos(0jJ 
sm(0j^ cos^0,j ^ATZsm(0,) +/ni'’cc«(0,^^ — sm|0j) sin^0,) ^ZaZsm(0,J 
+ /ji'Zcos|0,j j — cos|0j j ^ ArZsm(0,^ + Z^'Zcos^0,J) j, 

[sin^0,) ^AiZcos|0j) cos^0,) — /tjZcos(0jJ sin(0,^ + Ari’sm^0j^^ 

+ cos^0,j ^/jtZcos|0j^ cos( 0,^ — ZrZcos^0j) + ZrZsin^0jj J, 

sin(0,^ ^ATZsin(0j) cos|0,) — /nZsin^0j) sin^0T) — ArZcos|0jJ^ 

+ cos^0,) ^/jTZsm^0j^ cos^Qi) — ZTZsm(0j^ sm^0,) — A^?cos(0jJ 
sin(0T^ (ATZsin(0,) +/^'Zcos|0t) j +cos^0,) ^ATZsm(0,^ + Z;i’Zcos^0t) ) jj 


exploit symmett}' in inertia matrix... 

> MV != Aj 7 : Ar/ := /jo/: ZnV == /iz/: ///: 

> '/yx2 — hy2:/o(2-/jo2:/::y2-/yz2:/22: 

Vlampiilator Ineraa Matrix 

Translational Energy 

> MtI Multiply {J'ranspose{SubMatrix{J/l, [1.3], [ \ ..n])), m^-SitbMarrix(J//, [13], [1 

•^])); 

> Mt2 •= Multiply ^Tratispose{SubMatrix(J/2, [1 .3], \ \ ..n\)), m^-SubMatrix(J/2, [1.3], jl 


Mt2:= 


t, ^ -^7j sm(0j) - y (Z,sin(0j + 0,) - y a, sin(0j - 0,) + cos(0j) < 

- y Zt, sin(0j + 0,) - y lx, sm(0j - 0,) + y cos(0j - e,) - y A; cos(0, 
+ 0,) + cos(0j) A, j + m, cos(0j) + y i?, cos(0j — 0,) + y a, cos(0j 
+ 0,) + sm(0j) ^, + y cos(0j - 0,) + y lx, cos(0, + 0,) - y Zj’ sin(0j 


( 
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+ 0,) + +^“i(0i) ^" 2 ] . [-^iSin(0i) - y ^2®“(®1^®2) 

- y sin(0j - Gj) + cos(0j) ^2 - y A, sm(0j + 0,) - y /x, sm(0, - 0,) 

+ y A; cos(0j - 0,) - y cos(0j + 0,) + cos(0j) A, j w, ^ - y a, sm(0j + Gj) 
+ \a, sin(0i - Gj) - y /t, sin(0j + 0,) + y lx, sm(0j - 0^) - y A; cos(0j 

“®2)“ T^2‘^®*(®1 + ®2)) + y''2‘^°®(®1 “®2) T'^2‘^°K®1 

+ 0,) + sm(0j) + y A; cos(0j - 0,) + y Zu, cos(0j + 0,) - y /j' sm(0j 

+ 62) y ^2 “ ®2) + ^'2) ^^2 ( ■ y ^2 “ ®2) + y ^2 

+ ®2) “ y ^ “ ®2) + y + ®2) “ y ^2 ^“(®i ■'■ ^2) 

- y A;,sin(0j-0,) j , 

j^-^jSm(0j) - y ^7,sm(0,+ 0,) - y ^7,sm(0,-0,) +cos(0j)< 

- y Zv, sin(0, + 0,) - y lx, sm(0j - © 2 ) + y ^2 cos(0, - 0,) - y Aj, cos(0j 

+ 0,) +cos(0j) Iz^ m, J-y ^7,sm(0j + 0,) + y sin(0j - 0,) - y Zl 2 Sin( 0 j 

+ ® 2 ) + y ^ " ® 2 ) " y ^2 “ ® 2 ) “ y ^2 ® 2 ) ] 


r / V 1 / V 

1 / V 


1 <7, cos(0j) + ^ a, cos(0j - 0,) 

+ y d',COS(0j + 0,j 

+ sin^Gj) 


+ y Zi, cos(0j - 0,) + y Z^ cos(0j + 0,) - y Aj, sm(0, + 0,) + y Ij’, sm(0, 

- 0,) + sm(0j) Zr, j a, cos(0j - 02 ) + T ^2 «^‘»(0, + 02 ) - y Z^ cos(0j 


1 , / 

1 , / 

1 . • , 

\ ( 

y Z^COs(0j + 0,) 

- y ^jSin(0j + 0,) 

- y ^2^“(®i-02) 

)’'"4 


- y sm(0j + 0 ,) + y a, sin(0j - © 2 ) - y ^2 ^ 2 ) y 

- 0,) - y Aj cos(0j - 0,) - y Aj cos(0j + © 2 ) j + ^ - y ^2 “ ^ 2 ) 
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+ y ^7, cos(0j + Qj) - y A, cos(0j - 0,) + j Ix^ cos(0j + 9,) “ ^ Aj sm(0j 
1 Y ’I 

_> 

iotatiooal Energ\^ 

> A/rJ •■= Multiply^Tramposei,SubMatrix{ MI, [4 ..6), [ 1 .. 27 ))), Multiph\ll SubMarr/x(MJ, 

[4. 6], [1.:/;]))); 

M'J 0 
0 0 

> M'J■■= Multiply^Trampose{SubMatrix{M2. [4..6J, [ 1 .7/j)), A/ultiph'(IJ SubMarrix(M2. 

[4.6J,[1.:/;]))); 

Mr2\= ^ATi’sin^0,) +/rj^cos(0,^^ +cos^0,^ ^^^ 5111 ( 0 ,^ 

+ ,Aj7'i’cos|0Tj). (sm(0,) (ATi’cos(0j) cos^0,) — Ai?cos^0j) sin(0T) 

+ Ar-’sin^0j j +cos^0,) (/jx7cos|0j^ cos(0,^ — ATJ'cos^0jj sin^0,) 

+ ,^ri.^sin(0j))) “ (^ 111 ( 0 ^) (ArJ’sin(0j) cos(0t) — ATLi’sm|0j^ sm(0T) 

— ArJ’cos^0jJ +cos^0,J ^.^xJ’sm^0j^ cos(0,^ ^j“(0->) 

— ,^rJcos( 01 ))) cos( 0j) j, 

^cos^0j^ cos^0,^ ^Ax?sm^0,J +/9i^cos(0,^ ^ 

— cos(0j) sm(0,) (/jx.*.^sin(0 T) + Ati^cos(0 ,j) +sin(0j) (ArJsm(0,) 

+ Z^ Aos(0,) j j — cos^0jj (sin^0jj cos^0,) ^Aii?sin^0,^ + Ai'i’cos^0, j j 

— sm(0j) sm(0,) (,^x?sm(0,j + A,74?cos(0,)) — cos(0j) (Ar.Jsin(0,^ 

+/n Aos(0,) j j, sm^0j^ | (cos(0j) cos^0,j (ArJcos(0jj cos(0,) 

— /n'i’cos(0j j sm(0T) + ArJ’sin(0j)) — cos(0j) sm^0,j ^Ax.icos(0j) cos(0,) 

— A,Ti’cos^0j) sin(0,) +^^$111^0^11 + ^“(®i) (Ai-j’cos(0j J cos(0,) 

— 2^i'’cos^0j) ‘.in(0,) + AA’sm(0j))) ^“(0i) 

— (cos(0j) cos(02) (A2'-.^sm(0jJ cos(0,) — A;i’J’sin^0j) ^ 111 ( 02 ) — /»zJcos^0j)) 

— cos(0j) sm(0,) (/jTj'sin(0j) 005 ( 0 ^^ — A;»'i’sm(0j) sm(0,) — A2:<?cos(0j)) 

+ sin(0j^ (ArJ’sin^0j) cos^0,) — i^'J’sin^0j sm^0,j — Azi’cos|0j^^ J cos(0j^^ 

— cos(0j) ((sm(0j) cos(0t) (Ar?cos^0jJ cos(0,) — ArJ’cos^0j) sm(0.,) 

+ Ar.Jsin^0j) — sin(0jj sm(0,^ (,^.Ti’cos^0j j cos|0,^ — .Ai'J’cos(0j j sm(0,^ 
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+ ) — cos(0j) |Z:a.?cos(0j) cos(0,) — Z 7 'i’cos( 0 j) sm(0T) 

+ ZrZsm^0j )) (-^-5111(0^1 cos^0,^ 

— /riZsm^0j) sin^0,j — ArZcos^0jj j — sin^0j) sin^0,) ^^TZsm|0jJ cos(0,) 
— ^1^5111^0^ sm^0,) — ^rZcos|0jj j — cos^0j ^^ 1 ^ 511110 ^^ cos(0,) 
-Z7i?sin(0j) sm(02) - Z-?cos(0,))) cos^0j)) j] 


> 

Total Energ>' 

[> M■= combine{ Mtl + Mt2 + 2frl + Mr2 ): 
Veclocity Coupling Vector 
> V-= Fector(n,0): 
for/from 1 to/? do 
for/from 1 to//do 
for A'-from 1 to//do 


r ■•= V.+ eia/ 

t t 


end do: 
end do; 
end do: 

combine^ F), 
d 


v'. 


a Id 

- M. -i--J/^ 

00, 2 d0. 

i t 


•to -(0, 

J * 


a / rr 1 

yMfl + — t7j h\ sin^ 0, ) + — lxx2 /?, cos^ 0, j 


•¥lm,a^bz, cos(0,) — ///, Ix^ l\% sm(2 0,) + — -y /«, l}^ cos(2 0,) + /«, 

/^ + y //A a; + y ///^ ^; + y Ax?sm(2 0,) - /w, tT, sin(2 0,) 

+ //A <7, it, cos(2 0,) + y ^;i'Zcos(2 0,) — y ArJcos(2 0 ,) + AyJ + y AtZ+ //a 

+ //A /4 + -w, <■ + y ///, a; cos(2 0,) + y -w, Zi; cos(2 0,) + y 2^Zsm(2 0,), 

-m, <4 i}., cos( 0 ,) — ftt, Z, it, sm( 0 ,) — m, Iz^ sm( 0 T) — /«, k-, cos( 0 ,) 

- /»/, d, t7, sm(0,) - /?A d, it, sin^0,) + /i2'Zcos(0,) + ZtrZsm(0,) j. 


JZ:vJ’sin(0,) + Z7’icos(0,) - A’-, cos(0,) ~ sm(0,) 
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w, Z:, ^7, sin^Gj) - w, Z:, A; cos(0,) - w, </, d', sin(0,) - //z, </, Zi; sin(0,). 


,M/ 


2 OT, <7, Z^ + Z^ + d-,- + 77Z, ) ®i “: + — 

k - 

r r 1 1 

+ liti^d^t^ — l a^ A; sin(0,J + — lxx 2 + 1 a^ cos(0, j 

+ 2 /«, i7j Zi; cos(0,) - !x^ ly^ sm(2 0,) + z7, Zi; - y /«, ly^ cos(2 0,) + y /w, 

Zi; + y 77A ^2 y ^2 y ATi’sm(2 e,) ^2 ^2 ®;) 

y nua^bL, cos(2 0, J + y ^j.i^cos(2 0,) — y Ar. cos^2 0, j + Ai7+ y M’2-\- in, 

d'j* + 7«, Zi^ + 777, 7,' + y TZi, ^t; C0s( 2 0,) + y 7%, Z»^ C0s(2 0,) + y ^75111(2 0,), 

-777, cos(0,) — 7W, Zr, Zi; sin(0,) — 777, Iz^ 77, sm^0, j — 777, Iz^ ly^ cos(0,) 

- 777,7/, 77, sm(0,) - rn^ d^Lx^ ^“^(^2) "*■ + Zir.^sm^G,) J, 

JZh-^^sm^0,) + Zn'i’cos(0,) - <^ 0 ^( 97 ) “ f^K I--, sm(0,^ 

- 777, Zr, 77, sin(02) - 777, Z, A; cos(0,) - 777, </, 77, sm(0 ,) - 777, tZ, Zi; sm(0,), 

2 TV, 77, Zc, + 777, Zt; + 777,77^ -t- 777, ly + 7^^]]) ) aj*|, 




M/ + 


2 777, tT!, ZZ, - 2 777, 77j 


3,) + yZ:,Z 


-I- 2 777, 77j 77, COS^0,) + 2 777, 77j Zi; COS(0,) - 777, Zc, A; Sin^2 0,) -I- 777, 77, Zc, — y 777, 
AJ COS(2 0,) + y Z»^ + y 777, 77,* -H y 777, A^ -|- y ^JC2%\Xl{2 0,) 

- 7>7,77, sin^ 2 0,) + 777 , 77 , Zi, cos( 2 0 ,) + y ZtZcos( 2 0,) - y AvZcos(2 0,) 
-I- AyJ + y M2+ 777 , 77 ^ + 777, Zz; + 777 , 7 /,* + y 777, 77^ COS| 2 0,) + y W, 

Zx; cos(2 0,) + y /iTZsm(2 0,). - 77 ^ tZ, cos(0,) - 777 , Zz, Zi; sm(0,) 

- 777, Zr, 77, sm(0,) - 777, Zt^ A; cos(0,) - 777, tZ, 77, sm(0,) - 777, tZ, lx, sm(0,) 

-I- AzZ’cos( 0, ) + ArZsin^ 0, ) J, 
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+ Z7'i’cos(0,^ - ^°^{%) ~ ^-■, sm|0j) 

- Z:, a^ ~ ^2 ^2 cos(e,) - w, ^2 ^2 ®‘“(®:) “ ^2 ^*2 ^“(^2)’ 

2 m^a^/x:^ + + /«, a^ + ■*■ ^ ~~ |^^■dy + 

1 1 -1 

2ffi,d^lz^ — l m^ fl'j A; sm|0,) + y /ix?+ 2 ^Zj cos^0,) -^-Im^a^ Zt, cos(0,) 

- -w, Zr, Aj sin ^2 0 ,) + w, ^2 ^*2 “ T '^^2 ^2 ^2) ■*■ y '^^2 ^*2 ■*■ T ''^2 ^^2 

+ y A; + y .^a-?sm |2 0,) - -w, a^ ly^ sm (2 0 ,) + w, a^ A; cos (2 0 ,) 

+ y A>L^C0S(2 0,) — y AT-2C0s( 2 0,) + ^,T/+ y -^y2+ /W, + /«, /if + /«, 

+ y w, cos(2 0,) + y m, /i' cos(2 0,) + y /iTJ’sm(2 0,), d^ ly-, cos(0,) 

- m^lz^bc^ ^“(^2) “ ^2 ^“(^2) “ ^2 ^~2 ^'2 ~ "'2 ^2 ^2 ^“(^2) 

- />z, </, Zc, sin( 0^) + Ar.^cos^ 0-,) + Arising 0.,) j, 

jAni^sm(0,) + Zn'J’cos(0,) - //z, d^ A; cos^0,) - /w, /r, Zu, sm|0,) 

- Z, (7, sm( 0,) - m^ Iz^ A; cos( 0-,) — ■«, sm(0,) - /«, Z, Zr, sm(0,), 

2 /«t, zz, Zc, + /«, Zif + /«, zz, + z», Af + ) ~ y + 

1 2,1 

2m^d^lz^ — 2m^a^ A, sin|0,) + y /ix?+ 2 /«, zZj zz, cos|0t) + 2 /«, zZj Zc, cos(0,) 

- w, A, A^ sm (2 0 ,) + zzz, ^2 “ T ”’2 ^2 ^ ^2) y '^^2 T ^ 

+ y m^ A; + y ^TZsm^2 0,) - zzt, zz, /j; sm(2 0, j + zz/, zz. A, cos(2 0 ,) 
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+ y ^;n^cos(2 0,) — y Ai.2cos(2 0-,) + y a[ + m,/£; + m, 

+ y OT, iz; cos(2 0,) + y OT, cos(2 0-,) + y /n'i’sin|2 0-,), -rtLy d^ fy'^ cos|0,) 

— sin^0ij — /«, /z, sm|0,) — /«, /z, ^^cos|0,) — nu, d~, sm(0,) 

- w, d^ Zc, sin^0,) + ^r_^cos^0j j + Arising 0^) j, 

[^vism(0,) + Z7 .?cos(02 ) - m,d^Jy^ “ '^'2 ^2 ^*“(^ 2 ) 


/w, Z-, < 7 , sm( 02 ) - -w, Z, A; cos( 0 ,) - w, < < 7 , sm( 0 ,) - d^_ li\ sm( 0 ,). 


2 <7, ZC, + /W, Zc; + /«, <7, + /«, + ZrZjj) ) COj to, — y 


1.2 


d 

30, 


MJ 


Ini^d^t^ — l m, <7j A; sm(0T^ + y /ix7+ 2 /«, ^Zj <7, cos(0-, J 

1 1 1 

+ 2m^a^tc, cos(0-,) — m, Zt, /}\ sm(2 0-,) + 7fu,a^lXy — ^ m, cos(2 0,) + y /«, 

Zt; + y Z7A + y ^2 + y M2sm{2 02 ) - zz^ ^2 ^2 ^^(2 ^ 2 ) 

m^a^bc, cos^2 0^^ + y ZiZcosp 0,^ — y ArZcos|2 0,^ + hyl-V y hy2-V w, 

cr^ + Zy + d‘ + y -w, cos(2 0^) + y w, Z; cos(2 0,) + y An?sm(2 0,), 

-fft, d^ ly\ cos( 0 ,) — /w, Zz, LXy sin( 0 T) — /«, Zz, 5111^0,) — Z, cos( 0 t) 

- zzz, d^ <7, sin( 0 ,) — m^d^Lx^ + /jz:-'’cos( 0 , j + /tr-^sm( 0 ,) j, 

[Z:iz’sm(0,J + Z7'i’cos(0,) - m^d^ly^ ^°®(®2) “ ^■> ®“(®->) 
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< 7 , sm 




m , t , h \ cos 


(®:) “ '‘^2 ®“(® 2 ) ~ '"l ‘^2 ®“(® 2 )’ 


2 m, a^ bc, + m^ l:q + f/i, a; + «, hi + ^-^]|) ) ": “i + y 


2 . 1 


d 

ae. 


MrJ 


Ini^d^t^ — l ni, Aj, sm^e^ j + — hx2-v 2 /«, a~, cos^G^ j 

+ 2/fL,a^tc^ cos(0T^ — /«, /j;:, sm(2 0-,) ni^a^tx^ — y /«, cos(2 0,) + y w, 


/i; + y ^7* + y ^2 ■•■ y ATi’^in(2 0,) - /«, ^7, sm(2 0,) 

-\- nua^Ix^ cos|2 0,^ + y ^i.Jcos^2 0,) — y Ar-'’cos(2 0,) + hyl-\- y hy2-\- nu, 

d'j' + ;«, /^ + /«, i/j* + — m^ct; cos^2 0,) •¥ —m^tc, cos(2 0,) + y ^L^sin(2 0,), 

-/>*, h\ cos(0,) — /W, Ix^ sm^0T J — /«, /z, sin(0,) — /«, Iz^ cos^G,) 

- d^ sm(0,J — m^d^Ix^ + ^2v2cos(0,) + ifc2?sin(0,) j, 

[/nz’sm(0,) + Z^'J’cos^G,) - fti^d^Iy^ “ -’’A sm(0,) 

— w, Z, < 7 , sin( 0 ,) — /w, /z, A; cos( 0 ,J — /«, Z, a^ sm( 0 ,^ — //z, d^ Ix^ sm( 0 ,^, 

2 m,a^lx,y m^bq^ w, dl + w, Aj + ^-^]]) ) to^U 
Gra\irv Vector 

r ' 2.2 

> <?:= Fecror{n,Q ): 

^,^== r6r/i77^[0,0,-^rl): 

/ := 1: 

for/fiom 1 to 77 do 

<5; != G.-m-Multiply[Transpose^g^^, Vectori^ \SubMatrix{JIl, [1 ..3], [/])])): 

end do: 

/• := 2: 

foryfrom 1 to 77 do 

:= Gj-m-Multiply[Transpose^g^^, Vector{\SubMatrix{J12, (1 -3], [y]) ]) J; 
end do: 

a 

0 


^2 ('^2 ‘^ 0 S (® 2 ) ■'■ <^ 0 S (® 2 ) -^2 ~ ^“(® 2 ) ^ 2 ) 

Assemble the d^iiaimc equations 
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> T ■■= F^A?r(/;,0) : 

T != MuItiph i^M Vecfor(^ [a,, a, |)) + C?+ 

T ■•= comb/ne{T) : 

T != collect\l, [oj, : 

> /■ := T 

1 1 ’ 

7 ; ;= ( -m, d, a, sm(0,) - /«, /z, ^7, sm(e,) - m, Iz^ lx, ~ ^'2 ^’2 

— /«, d, l)\ cos^G,) + -&:J’sin^0T) + — /«, d, Lv, sm(0,)) a, + f ArJ 


+ 2 m, d, Iz, + i^T7sm|2 0,) ■¥ m, Z.“ + y ly}'2-\- 2 Zij 

+ y l\y2zo%{2 0,) - y m, ly\ cos(2 0,) + y -w, cos(2 0,) + m, a, lx, cos(2 0,) 

■) 1 •> 

- m, ^2 sm(2 0,) + m, d‘ + — m, lx; cos(2 0^) + 2 m, Lx, €05(0,) 


— m, be, fy', sin^2 0,) + a‘^ -\- m^b^ + m^lr^ — 2 m, fy, sin^G,) 


— y Ai.2cos( 2 0,) + 2 /w, <7j a, cos( Q,) + y ^'^2 + m, a, be, + y /w, <7^ + y m, 

A; + ^ m, A; j Oj — 03* Ac7sm(0,) — co^ co, jn, b^ sin(2 0,) + co* Ar7cos(0,) 

+ cOj CO, m. A; sm( 2 0, ) + 2 cOj co, /i;j.7cos^ 2 0,) — 2 co^ co, /n, be, A; cos( 2 0,) + 

•> 

(o‘/^^/, ^;,sm(0,) — cOj (D,^T7sm(2 0,) + cOj to, Ar7sin(2 0,) + 

(o‘, m, Iz, fy, sm(0,) - o)^ w, d, be, cos(0,) - cOj to, m, eT, sin(2 0,) 


7 

- 2 tOj to, 7n, t7j lx, sin(0,) - to' m, t, be, cos^G,) - 2 tOj to, m, a, fy, cos^2 0,) 

7 

to' m, d, a, cos(0,) — 2 tOj to, m, a. Lx, sm^2 0,) - 2 tOj to, m, a, sin^G,) — 


to' m, Iz, cos( 0, ) - 2 to^ to, fy^ cos( 0, ) 

> CodeGenerarion\ 'MatLad ) (7J); 

cg2 = (-in(2) * d(2) * a(2) » sin(theta(2 ) ) - m(2) » lz(2) • a 

(2) * sin(theta(2)) - m(2) » lz(2) * lx(2) * sin(theta(2)) - m 

(2) * lz(2) * ly(2) * cos(theta(2)) -m(2) • d(2) * ly(2) * cos 

(theta(2)) + Ixz2 * sin( theta( 2) ) + Iyz2 * cos( theta(2 )) - in(2) 

* d(2) » ix(2) • sin(theta(2))) * alpna(2) + (lyyl + 0.2el » m 
(2) * d(2) * lz(2) + Ixy2 * sin(0.2el * theta(2)) + m(2) * a(l) 
" 2 + m(2) » lz(2) " 2 + Iyy2 / 0.2el + 0.2el * m(l) * a(l) • 
lx(l) + Iyy2 * cos(0.2el * theta(2)) / 0.2el - in(2) * ly(2) 

* cos(0.2el * theta(2)) / 0.2el + m(2) * a(2) * 2 » cos(0.2el 
theta(2)) / 0.2el + m(2) * a(2) * lx(2) * cos(0.2el * theta(2)) 
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CM 




- m(2) * a(2) » ly(2) • sin(0.2el • theta(2)) + m(2) * d(2) ^ 2 

+ m(2) * lx(2) * 2 » cos(0.2el » theta(2)) / 0.2el + 0.2el » m 
(2) * a(l) * lx (2) » cos(theta(2)) - m(2) * lx(2) * ly(2) * sin 

(0.2el * theta(2)) + m(l) * a(l) " 2 + in(l) * lx(l) " 2 + m(l) 

* lz(l) * 2 - 0.2el * m(2) * a(l) » ly(2) * sin(theta(2)) - 

Ixx2 * cos(0.2el * theta(2)) / 0.2el + 0.2el * in(2) * a(l) * a 

(2) * cos(theta(2)) + Ixx2 / 0.2el + m(2) * a(2) * lx(2) + in(2) 

» a(2) " 2 / 0.2el + m(2) * lx(2) ^ 2 / 0.2el + m{2) * ly(2) ■' 

2 / 0.2el) » alpha(l) - omega(2) * 2 * Iyz2 * sin(theta(2)) - 

omega(l) * omega(2) * ni(2) * lx(2) * 2 ♦ sin(0.2el * theta(2)) 

+ omega(2) ^ 2 » Ixz2 * cos(theta(2) ) + oniega(l) * omega(2) * m 
(2) * ly(2) " 2 * sin(0.2el * theta(2)) + 0.2el * omega(l) * 

omega(2) * Ixy2 * cos(0.2el * theta(2)) - 0.2el * omega(l) » 

omega(2) * in(2) * lx(2) * ly(2) * cos(0.2el * theta(2)) + omega 
(2) " 2 * m(2) • d(2) * ly(2) * sin(theta(2)) - omega(l) * 
omega(2) * Iyy2 * sin(0.2el * theta(2)) + omega(l) * omega(2) * 

Ixx2 • sin(0.2el * theta(2)) + omega(2) ^ 2 *m(2) * lz(2) » ly 

(2) * sin(theta(2)) - omega(2) * 2 » m(2) * d(2) * lx(2) * cos 

(theta(2)) - omega(l) » omega(2) * m(2) * a(2) * 2 * sin(0.2el 

* theta(2)) - 0.2el • omega(l) » omega(2) * m(2) » a(l) • lx(2) 

* sin(theta(2)) - omega(2) ' 2 * m(2) * lz(2) • lx(2) * cos 
(theta(2)) - 0.2el » omega(l) * omega(2) * m(2) * a(2) * ly(2) 

* cos(0.2el * theta(2)) - omega(2) * 2 *m(2) * d(2) * a(2) * 
cos(theta(2)) - 0.2el » omega(l) * omega(2) * m(2) * a(2) * lx 
(2) * sin(0.2el • theta(2)) - 0.2el * omega(l) • omega(2) * m 
(2) * a(l) * a(2) * sin(theta(2)) - omega(2) * 2 * m{2) * lz(2) 

* a(2) * cos(theta(2)) - 0.2el • omega(l) * omega(2) * m(2) •a 

(1) * ly(2) * cos(theta(2)) ; 

> /■ := T 

^2 2’ 

T; ;= ( + OT, i7, cos^Gj) + ^ + 2 ^7, + /«, 

+ + ffi, A^'j a, + ( -/n, sin^G,) — m, /r, Zc, sin(G,) 

— Zr, A:, cos|G,^ — /«, d^ h\ cos(G,^ + A?.7sm(G,) + As’Z’coslG,) 

2 1 2 1 2 

— m^d^lx^ sm(G^)) a, + (uj /c/, <7, A', cos(2 G,) — — (oj j5:i.?sm(2 G^) + y coj 

Zt;sin(2G,) + (Oj /«, iTj A, sin(2 G,) 4-cojw, <7j ^7, sm(G,) + co*<7 j Zc, sin^G^) + 

ooj m, Ix, A’ cos( 2 G,) + ojJ cos( 9, ) “ “5 -^cos( 2 G^ ) + y 

ojj Ari’sm(2 G,) - y toj Aj sm(2 G,) + y (oj m, a; sm(2 G,) 

> CodeGe}7erarion['MaTlad]^T^'^\ 

cg3 = (-m(2) * ly(2) * sin( theta( 2)) + m(2) * a(2) » cos(theta 

(2) ) + m(2) * lx(2) * cos(theta(2))) * gc + (Izz2 + 0.2el * m 
(2) * a(2) » lx(2) + m(2) * a(2) '' 2 + m(2) * lx(2) * 2 + m(2) 

* ly(2) '• 2) * alpha(2) + (-m(2) * d(2) * a(2) » sin( theta( 2) ) 

- m(2) * lz(2) • a(2) • sin(theta(2)) - m(2) * lz(2) * lx(2) * 

sin(theta(2)) - m(2) * lz(2) * ly(2) * cos( theta( 2)) - m(2) * d 
(2) * ly(2) * cos(theta(2)) + Ixz2 » sin(theta(2)) + Iyz2 * cos 
(theta(2)) - m(2) * d(2) * lx(2) * sin( theta( 2))) » alpha(l) + 

omega(l) * 2 • m(2) * a(2) » iy(2) » cos(0.2el » theta(2)) - 

omega(l) ^ 2 * Ixx2 * sin(0.2el * theta(2)) / 0.2el + omega(l) 

^ 2 • m(2) • lx(2) '' 2 * sin(0.2el * theta(2)) / 0.2el + omega 
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(1) ^ 2 * m(2) » a(2) * lx (2) * 3in(0.2el * theta(2)) + omega 

(1) 2 * m(2) » a(l) * a(2) * sin( theta( 2) ) + omega(l) * 2 * m 

(2) * a(l) • lx (2) * 3in( theta( 2) ) + omega(i) '' 2 * m(2) * lx 
(2) * ly(2) * co3(0.2el * theta(2)) + omega(l) * 2 » m(2) * a 

(1) * ly(2) * C03(theta(2)) - omega(l) * 2 * Ixy2 * cos(0.2el » 
theta(2)) + omega(l) * 2 * Iyy2 * 3in(0.2el * theta(2)) / 0.2el 

- omega(l) * 2 * m(2) » ly(2) * 2 » sin(0.2el * theta(2)) / 
0.2el + omega(l) '' 2 * m(2) » a(2) '' 2 » 3in(0.2el * theta(2)) 

J 0.2el; 

> 

_> 

> 

> CodeGeneraTion\ 'Matla& ] ( j); 

cg4 = lyyl + (2 * m(2) » d(2) * lz(2)) + Ixy2 * 3ln((2 * theta 

(2) )) + (m(2) * a(l) '' 2) + (m(2) * lz(2) * 2) + Iyy2 / 0.2el + 
(2 * m(l) * a(l) * lx(l)) + Iyy2 * co3((2 • theta(2))) / 0.2el 

- m(2) * ly(2) * 2 * cos((2 * theta(2))) / 0.2el + m(2) * (a(2) 

^ 2) * co 3((2 » theta(2))) / 0.2el + m(2) * a(2) * lx(2) * co3( 
(2 * theta(2))) - m(2) * a(2) • ly(2) * 3in((2 » theta(2))) + 
(m(2) * d(2) ' 2) + m(2) * (lx(2) 2) » co3((2 * theta(2))) / 

0.2el + 0.2el • m(2) * a(l) • lx(2) * co3(theta(2)) - m(2) • lx 
(2) * ly(2) * sin((2 * theta(2))) + (m(l) » a(l) " 2) + (m(l) * 
lx(l) " 2) + (m(l) * lz(l) " 2) - 0.2el * m(2) * a(l) • ly(2) * 
3in(theta(2)) - Ixx2 * co3((2 » theta(2))) / 0.2el + 0.2el » m 
(2) * a(l) * a(2) * co3(theta(2)) + Ixx2 / 0.2el + (m(2) * a(2) 

* lx(2)) + (m(2) * a(2) " 2) / 0.2el + (m(2) * lx(2) ^ 2) / 
_0.2el + m(2) * ly(2) '■ 2 / 0.2el; 

> CodeGeneration\'Marlad\\^ 

cg5 = -m(2) * d(2) » a(2) • 3in(theta(2)) - m(2) * lz(2) * a(2) 

* sin(theta(2)) - m(2) * lz(2) » lx(2) * 3in(theta(2)) - m(2) » 
lz(2) * ly(2) • co3(theta(2)) - m(2) * d(2) * ly(2) * co3(theta 
(2)) + Ixz2 * sin(theta(2)) + Iyz2 » co3(theta(2)) - m(2) ♦ d 

_(2) * lx(2) * 3in(theta(2)) ; 

> 

8 

> CodeGemration\'Matlad\\^M^ 

cg6 = -m(2) * d(2) • a(2) * 3in(theta(2)) - m(2) * lz(2) * a(2) 

* 3in(theta(2)) - m(2) * lz(2) » lx(2) » 3in(theta(2)) - m(2) * 
lz(2) * ly(2) * C03(theta(2)) -m(2) * d(2) * ly(2) * co3(theta 
(2)) + Ixz2 * sin(theta(2)) + Iyz2 » cos(theta(2)) - m(2) • d 

^(2) * lx(2) * sin(theta(2)) ; 

> CodeGe?ierarion\’Mar/aff\^.\£, 

cg7 = Izz2 + 2 * m(2) *3(2) * lx{2) + m(2) * a(2) * 2 + m(2) » 
^lx(2) '' 2 + m(2) * ly(2) * 2; 

_> 

> CodeCenerarion{ 'Mailad ] (); 

.cg8 = 0 ; 

> CodeGemration\ 'Marlad )(<?■>); 

cg9 = m(2) * (a(2) * cos(theta(2)) + lx(2) * cos(theta(2)) - ly 
_(2) * sin(theta(2)) ) * gc; 

_> 

> CodeGemranon\ 'Marlad ] ( ; 

cglO = (-0.2el » m(2) • a(l) * lx(2) * sin ( theta( 2)) + 0.2el * 
Ixy2 * cos(0.2el * theta(2)) - Iyy2 * sin(0.2el * theta(2)) - 
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0.2el • m(2) * a(l) * ly(2) * cos(theta(2)) - 0.2el * m(2) » lx 
(2) * ly(2) * cos(0.2el * theta(2)) + Ixx2 » 3in(0.2el * theta 
(2)) - 0.2el * m(2) * a(l) • a(2) • sin(theta(2)) - in(2) * a(2) 
^ 2 * sin(0.2el * theta(2)) - m(2) * lx(2) ' 2 • sin(0.2el * 
theta(2)) + m(2) * ly(2) " 2 * sin(0.2el * theta(2)) - 0.2el * 
in(2) * a(2) * lx(2) * sin(0.2el * theta{2)) - 0.2el * m(2) » a 
(2) * ly(2) * cos(0.2el * theta(2))) * omega(l) * omega(2) + (m 
(2) * lz(2) * ly(2) * sin(theta(2)) + m(2) » d(2) » ly(2) * sin 
(theta(2)) - m(2) * lz(2) * lx(2) » cos(theta(2)) - m(2) * Iz 
(2) * a(2) * cos(theta(2)) - m(2) * d(2) * lx(2) * co3(theta(2) 
) + Ixz2 * cos( theta( 2)) - Iyz2 * sin(theta(2)) - m(2) » d(2) » 
.a(2) * cos( theta( 2))) » omega(2) * 2; 

> CodeGenerar/on\ 'Marlad ] ( ); 

cgll = (in(2) * a(l) * lx(2) * sin(theta(2)) - Ixy2 » cos(0.2el 

* theta(2)) + Iyy2 * 3in(0.2el » theta(2)) / 0.2el + m(2) * a 

(1) * ly(2) * cos(theta(2)) + m(2) * lx(2) • ly(2) * cos(0.2el 

* theta(2)) - Ixx2 * sin(0.2el » theta(2)) / 0.2el + m(2) * a 

(1) * a(2) • sin(theta(2)) + m(2) * a(2) " 2 * sin(0.2el * 

theta(2)) / 0.2el + in(2) * lx(2) * 2 * sin(0.2el * theta(2)) / 
0.2el - m(2) * ly(2) ^ 2 * sin(0.2el * theta(2)) / 0.2el + m(2) 

* a(2) » lx(2) » sin(0.2el » theta(2)) + m(2) • a(2) * ly(2) * 

cos(0.2el * theta(2))) * omega(l) * 2 + (m(2) * lz(2) * ly(2) * 
3in(theta(2)) / 0.2el + m(2) * d(2) * ly(2) * sin(theta(2)) / 
0.2el - in(2) * lz(2) * lx(2) * cos(theta(2)) / 0.2el - m(2) * 

lz(2) » a(2) * cos(theta(2)) / 0.2el - in(2) * d(2) * lx(2) » 

cos(theta(2)) / 0.2el + Ixz2 * cos( theta( 2)) / 0.2el - Iyz2 * 

sin(theta(2)) / 0.2el - m(2) * d(2) * a(2) • cos(theta(2)) / 

0.2el) * omega(l) * omega(2) + (-m(2) » lz(2) * ly(2) » sin 
(theta(2)) / 0.2el - m(2) * d(2) * ly(2) * sin ( theta( 2)) / 

0.2el + m(2) * lz(2) » lx(2) * cos( theta( 2)) / 0.2el + m(2) • 

lz(2) • a(2) * cos(theta(2)) / 0.2el + m(2) * d(2) » lx(2) * 

cos(theta(2)) / 0.2el - Ixz2 * cos(theta(2)) / 0.2el + Iyz2 * 

sin(theta(2)) / 0.2el + m(2) * d(2) * a(2) » cos(theta(2)) / 

^0.2el) * omega(2) * omega(l); 

> 
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